projects / carveJwlIkooP6JGAAIwe30JlM.git / blob
? search:
summary | shortlog | log | commit | commitdiff | tree
history | raw | HEAD
LPR - Walking
[carveJwlIkooP6JGAAIwe30JlM.git] / rigidbody.h
1 /*
2 * Copyright (C) 2021-2022 Mt.ZERO Software, Harry Godden - All Rights Reserved
3 */
4
5 /*
6 * Resources: Box2D - Erin Catto
7 * qu3e - Randy Gaul
8 */
9
10 #include "common.h"
11 #include "bvh.h"
12 #include "scene.h"
13
14 #include <math.h>
15
16 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty );
17 VG_STATIC bh_system bh_system_rigidbodies;
18
19
20
21 #ifndef RIGIDBODY_H
22 #define RIGIDBODY_H
23
24 /*
25 * -----------------------------------------------------------------------------
26 * (K)onstants
27 * -----------------------------------------------------------------------------
28 */
29
30 VG_STATIC const float
31 k_rb_rate = (1.0/VG_TIMESTEP_FIXED),
32 k_rb_delta = (1.0/k_rb_rate),
33 k_friction = 0.4f,
34 k_damp_linear = 0.1f, /* scale velocity 1/(1+x) */
35 k_damp_angular = 0.1f, /* scale angular 1/(1+x) */
36 k_penetration_slop = 0.01f,
37 k_inertia_scale = 8.0f,
38 k_phys_baumgarte = 0.2f,
39 k_gravity = 9.6f;
40
41 VG_STATIC float
42 k_limit_bias = 0.02f,
43 k_joint_correction = 0.01f,
44 k_joint_impulse = 1.0f,
45 k_joint_bias = 0.08f; /* positional joints */
46
47 VG_STATIC void rb_register_cvar(void)
48 {
49 vg_var_push( (struct vg_var){
50 .name = "k_limit_bias", .data = &k_limit_bias,
51 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
52 });
53
54 vg_var_push( (struct vg_var){
55 .name = "k_joint_bias", .data = &k_joint_bias,
56 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
57 });
58
59 vg_var_push( (struct vg_var){
60 .name = "k_joint_correction", .data = &k_joint_correction,
61 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
62 });
63
64 vg_var_push( (struct vg_var){
65 .name = "k_joint_impulse", .data = &k_joint_impulse,
66 .data_type = k_var_dtype_f32, .opt_f32 = {.clamp = 0}, .persistent = 1
67 });
68 }
69
70 /*
71 * -----------------------------------------------------------------------------
72 * structure definitions
73 * -----------------------------------------------------------------------------
74 */
75
76 typedef struct rigidbody rigidbody;
77 typedef struct contact rb_ct;
78 typedef struct rb_sphere rb_sphere;
79 typedef struct rb_capsule rb_capsule;
80 typedef struct rb_scene rb_scene;
81
82 struct rb_sphere
83 {
84 float radius;
85 };
86
87 struct rb_capsule
88 {
89 float height, radius;
90 };
91
92 struct rb_scene
93 {
94 bh_tree *bh_scene;
95 };
96
97 struct rigidbody
98 {
99 v3f co, v, w;
100 v4f q;
101
102 enum rb_shape
103 {
104 k_rb_shape_box = 0,
105 k_rb_shape_sphere = 1,
106 k_rb_shape_capsule = 2,
107 k_rb_shape_scene = 3
108 }
109 type;
110
111 union
112 {
113 struct rb_sphere sphere;
114 struct rb_capsule capsule;
115 struct rb_scene scene;
116 }
117 inf;
118
119 v3f right, up, forward;
120
121 int is_world;
122
123 boxf bbx, bbx_world;
124 float inv_mass;
125
126 /* inertia model and inverse world tensor */
127 v3f I;
128 m3x3f iI, iIw;
129
130 m4x3f to_world, to_local;
131 };
132
133 VG_STATIC struct contact
134 {
135 rigidbody *rba, *rbb;
136 v3f co, n;
137 v3f t[2];
138 float p, bias, norm_impulse, tangent_impulse[2],
139 normal_mass, tangent_mass[2];
140
141 u32 element_id;
142
143 enum contact_type type;
144 }
145 rb_contact_buffer[256];
146 VG_STATIC int rb_contact_count = 0;
147
148 typedef struct rb_constr_pos rb_constr_pos;
149 typedef struct rb_constr_swingtwist rb_constr_swingtwist;
150
151 struct rb_constr_pos
152 {
153 rigidbody *rba, *rbb;
154 v3f lca, lcb;
155 };
156
157 struct rb_constr_swingtwist
158 {
159 rigidbody *rba, *rbb;
160
161 v4f conevx, conevy; /* relative to rba */
162 v3f view_offset, /* relative to rba */
163 coneva, conevxb;/* relative to rbb */
164
165 int tangent_violation, axis_violation;
166 v3f axis, tangent_axis, tangent_target, axis_target;
167
168 float conet;
169 float tangent_mass, axis_mass;
170 };
171
172 /*
173 * -----------------------------------------------------------------------------
174 * Math Utils
175 * -----------------------------------------------------------------------------
176 */
177
178 VG_STATIC float sphere_volume( float radius )
179 {
180 float r3 = radius*radius*radius;
181 return (4.0f/3.0f) * VG_PIf * r3;
182 }
183
184 VG_STATIC void rb_tangent_basis( v3f n, v3f tx, v3f ty )
185 {
186 /* Compute tangent basis (box2d) */
187 if( fabsf( n[0] ) >= 0.57735027f )
188 {
189 tx[0] = n[1];
190 tx[1] = -n[0];
191 tx[2] = 0.0f;
192 }
193 else
194 {
195 tx[0] = 0.0f;
196 tx[1] = n[2];
197 tx[2] = -n[1];
198 }
199
200 v3_normalize( tx );
201 v3_cross( n, tx, ty );
202 }
203
204 /*
205 * -----------------------------------------------------------------------------
206 * Debugging
207 * -----------------------------------------------------------------------------
208 */
209
210 VG_STATIC void rb_debug_contact( rb_ct *ct )
211 {
212 if( ct->type != k_contact_type_disabled )
213 {
214 v3f p1;
215 v3_muladds( ct->co, ct->n, 0.05f, p1 );
216 vg_line_pt3( ct->co, 0.0025f, 0xff0000ff );
217 vg_line( ct->co, p1, 0xffffffff );
218 }
219 }
220
221 VG_STATIC void debug_sphere( m4x3f m, float radius, u32 colour )
222 {
223 v3f ly = { 0.0f, 0.0f, radius },
224 lx = { 0.0f, radius, 0.0f },
225 lz = { 0.0f, 0.0f, radius };
226
227 for( int i=0; i<16; i++ )
228 {
229 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
230 s = sinf(t),
231 c = cosf(t);
232
233 v3f py = { s*radius, 0.0f, c*radius },
234 px = { s*radius, c*radius, 0.0f },
235 pz = { 0.0f, s*radius, c*radius };
236
237 v3f p0, p1, p2, p3, p4, p5;
238 m4x3_mulv( m, py, p0 );
239 m4x3_mulv( m, ly, p1 );
240 m4x3_mulv( m, px, p2 );
241 m4x3_mulv( m, lx, p3 );
242 m4x3_mulv( m, pz, p4 );
243 m4x3_mulv( m, lz, p5 );
244
245 vg_line( p0, p1, colour == 0x00? 0xff00ff00: colour );
246 vg_line( p2, p3, colour == 0x00? 0xff0000ff: colour );
247 vg_line( p4, p5, colour == 0x00? 0xffff0000: colour );
248
249 v3_copy( py, ly );
250 v3_copy( px, lx );
251 v3_copy( pz, lz );
252 }
253 }
254
255 VG_STATIC void debug_capsule( m4x3f m, float radius, float h, u32 colour )
256 {
257 v3f ly = { 0.0f, 0.0f, radius },
258 lx = { 0.0f, radius, 0.0f },
259 lz = { 0.0f, 0.0f, radius };
260
261 float s0 = sinf(0.0f)*radius,
262 c0 = cosf(0.0f)*radius;
263
264 v3f p0, p1, up, right, forward;
265 m3x3_mulv( m, (v3f){0.0f,1.0f,0.0f}, up );
266 m3x3_mulv( m, (v3f){1.0f,0.0f,0.0f}, right );
267 m3x3_mulv( m, (v3f){0.0f,0.0f,-1.0f}, forward );
268 v3_muladds( m[3], up, -h*0.5f+radius, p0 );
269 v3_muladds( m[3], up, h*0.5f-radius, p1 );
270
271 v3f a0, a1, b0, b1;
272 v3_muladds( p0, right, radius, a0 );
273 v3_muladds( p1, right, radius, a1 );
274 v3_muladds( p0, forward, radius, b0 );
275 v3_muladds( p1, forward, radius, b1 );
276 vg_line( a0, a1, colour );
277 vg_line( b0, b1, colour );
278
279 v3_muladds( p0, right, -radius, a0 );
280 v3_muladds( p1, right, -radius, a1 );
281 v3_muladds( p0, forward, -radius, b0 );
282 v3_muladds( p1, forward, -radius, b1 );
283 vg_line( a0, a1, colour );
284 vg_line( b0, b1, colour );
285
286 for( int i=0; i<16; i++ )
287 {
288 float t = ((float)(i+1) * (1.0f/16.0f)) * VG_PIf * 2.0f,
289 s1 = sinf(t)*radius,
290 c1 = cosf(t)*radius;
291
292 v3f e0 = { s0, 0.0f, c0 },
293 e1 = { s1, 0.0f, c1 },
294 e2 = { s0, c0, 0.0f },
295 e3 = { s1, c1, 0.0f },
296 e4 = { 0.0f, c0, s0 },
297 e5 = { 0.0f, c1, s1 };
298
299 m3x3_mulv( m, e0, e0 );
300 m3x3_mulv( m, e1, e1 );
301 m3x3_mulv( m, e2, e2 );
302 m3x3_mulv( m, e3, e3 );
303 m3x3_mulv( m, e4, e4 );
304 m3x3_mulv( m, e5, e5 );
305
306 v3_add( p0, e0, a0 );
307 v3_add( p0, e1, a1 );
308 v3_add( p1, e0, b0 );
309 v3_add( p1, e1, b1 );
310
311 vg_line( a0, a1, colour );
312 vg_line( b0, b1, colour );
313
314 if( c0 < 0.0f )
315 {
316 v3_add( p0, e2, a0 );
317 v3_add( p0, e3, a1 );
318 v3_add( p0, e4, b0 );
319 v3_add( p0, e5, b1 );
320 }
321 else
322 {
323 v3_add( p1, e2, a0 );
324 v3_add( p1, e3, a1 );
325 v3_add( p1, e4, b0 );
326 v3_add( p1, e5, b1 );
327 }
328
329 vg_line( a0, a1, colour );
330 vg_line( b0, b1, colour );
331
332 s0 = s1;
333 c0 = c1;
334 }
335 }
336
337 VG_STATIC void rb_debug( rigidbody *rb, u32 colour )
338 {
339 if( rb->type == k_rb_shape_box )
340 {
341 v3f *box = rb->bbx;
342 vg_line_boxf_transformed( rb->to_world, rb->bbx, colour );
343 }
344 else if( rb->type == k_rb_shape_sphere )
345 {
346 debug_sphere( rb->to_world, rb->inf.sphere.radius, colour );
347 }
348 else if( rb->type == k_rb_shape_capsule )
349 {
350 m4x3f m0, m1;
351 float h = rb->inf.capsule.height,
352 r = rb->inf.capsule.radius;
353
354 debug_capsule( rb->to_world, r, h, colour );
355 }
356 else if( rb->type == k_rb_shape_scene )
357 {
358 vg_line_boxf( rb->bbx, colour );
359 }
360 }
361
362 /*
363 * -----------------------------------------------------------------------------
364 * Integration
365 * -----------------------------------------------------------------------------
366 */
367
368 /*
369 * Update world space bounding box based on local one
370 */
371 VG_STATIC void rb_update_bounds( rigidbody *rb )
372 {
373 box_copy( rb->bbx, rb->bbx_world );
374 m4x3_transform_aabb( rb->to_world, rb->bbx_world );
375 }
376
377 /*
378 * Commit transform to rigidbody. Updates matrices
379 */
380 VG_STATIC void rb_update_transform( rigidbody *rb )
381 {
382 q_normalize( rb->q );
383 q_m3x3( rb->q, rb->to_world );
384 v3_copy( rb->co, rb->to_world[3] );
385
386 m4x3_invert_affine( rb->to_world, rb->to_local );
387
388 m3x3_mulv( rb->to_world, (v3f){1.0f,0.0f, 0.0f}, rb->right );
389 m3x3_mulv( rb->to_world, (v3f){0.0f,1.0f, 0.0f}, rb->up );
390 m3x3_mulv( rb->to_world, (v3f){0.0f,0.0f,-1.0f}, rb->forward );
391
392 m3x3_mul( rb->iI, rb->to_local, rb->iIw );
393 m3x3_mul( rb->to_world, rb->iIw, rb->iIw );
394
395 rb_update_bounds( rb );
396 }
397
398 /*
399 * Extrapolate rigidbody into a transform based on vg accumulator.
400 * Useful for rendering
401 */
402 VG_STATIC void rb_extrapolate_transform( rigidbody *rb, m4x3f transform )
403 {
404 float substep = vg_clampf( vg.accumulator / k_rb_delta, 0.0f, 1.0f );
405
406 v3f co;
407 v4f q;
408
409 v3_muladds( rb->co, rb->v, k_rb_delta*substep, co );
410
411 if( v3_length2( rb->w ) > 0.0f )
412 {
413 v4f rotation;
414 v3f axis;
415 v3_copy( rb->w, axis );
416
417 float mag = v3_length( axis );
418 v3_divs( axis, mag, axis );
419 q_axis_angle( rotation, axis, mag*k_rb_delta*substep );
420 q_mul( rotation, rb->q, q );
421 q_normalize( q );
422 }
423 else
424 {
425 v4_copy( rb->q, q );
426 }
427
428 q_m3x3( q, transform );
429 v3_copy( co, transform[3] );
430 }
431
432 /*
433 * Initialize rigidbody and calculate masses, inertia
434 */
435 VG_STATIC void rb_init( rigidbody *rb )
436 {
437 float volume = 1.0f;
438
439 if( rb->type == k_rb_shape_box )
440 {
441 v3f dims;
442 v3_sub( rb->bbx[1], rb->bbx[0], dims );
443 volume = dims[0]*dims[1]*dims[2];
444 }
445 else if( rb->type == k_rb_shape_sphere )
446 {
447 volume = sphere_volume( rb->inf.sphere.radius );
448 v3_fill( rb->bbx[0], -rb->inf.sphere.radius );
449 v3_fill( rb->bbx[1], rb->inf.sphere.radius );
450 }
451 else if( rb->type == k_rb_shape_capsule )
452 {
453 float r = rb->inf.capsule.radius,
454 h = rb->inf.capsule.height;
455 volume = sphere_volume( r ) + VG_PIf * r*r * (h - r*2.0f);
456
457 v3_fill( rb->bbx[0], -r );
458 v3_fill( rb->bbx[1], r );
459 rb->bbx[0][1] = -h;
460 rb->bbx[1][1] = h;
461 }
462 else if( rb->type == k_rb_shape_scene )
463 {
464 rb->is_world = 1;
465 box_copy( rb->inf.scene.bh_scene->nodes[0].bbx, rb->bbx );
466 }
467
468 if( rb->is_world )
469 {
470 rb->inv_mass = 0.0f;
471 v3_zero( rb->I );
472 m3x3_zero(rb->iI);
473 }
474 else
475 {
476 float mass = 2.0f*volume;
477 rb->inv_mass = 1.0f/mass;
478
479 v3f extent;
480 v3_sub( rb->bbx[1], rb->bbx[0], extent );
481 v3_muls( extent, 0.5f, extent );
482
483 /* local intertia tensor */
484 float scale = k_inertia_scale;
485 float ex2 = scale*extent[0]*extent[0],
486 ey2 = scale*extent[1]*extent[1],
487 ez2 = scale*extent[2]*extent[2];
488
489 rb->I[0] = ((1.0f/12.0f) * mass * (ey2+ez2));
490 rb->I[1] = ((1.0f/12.0f) * mass * (ex2+ez2));
491 rb->I[2] = ((1.0f/12.0f) * mass * (ex2+ey2));
492
493 m3x3_identity( rb->iI );
494 rb->iI[0][0] = rb->I[0];
495 rb->iI[1][1] = rb->I[1];
496 rb->iI[2][2] = rb->I[2];
497 m3x3_inv( rb->iI, rb->iI );
498 }
499
500 v3_zero( rb->v );
501 v3_zero( rb->w );
502
503 rb_update_transform( rb );
504 }
505
506 VG_STATIC void rb_iter( rigidbody *rb )
507 {
508 if( !vg_validf( rb->v[0] ) ||
509 !vg_validf( rb->v[1] ) ||
510 !vg_validf( rb->v[2] ) )
511 {
512 vg_fatal_exit_loop( "NaN velocity" );
513 }
514
515 v3f gravity = { 0.0f, -9.8f, 0.0f };
516 v3_muladds( rb->v, gravity, k_rb_delta, rb->v );
517
518 /* intergrate velocity */
519 v3_muladds( rb->co, rb->v, k_rb_delta, rb->co );
520 v3_lerp( rb->w, (v3f){0.0f,0.0f,0.0f}, 0.0025f, rb->w );
521
522 /* inegrate inertia */
523 if( v3_length2( rb->w ) > 0.0f )
524 {
525 v4f rotation;
526 v3f axis;
527 v3_copy( rb->w, axis );
528
529 float mag = v3_length( axis );
530 v3_divs( axis, mag, axis );
531 q_axis_angle( rotation, axis, mag*k_rb_delta );
532 q_mul( rotation, rb->q, rb->q );
533 }
534
535 /* damping */
536 v3_muls( rb->v, 1.0f/(1.0f+k_rb_delta*k_damp_linear), rb->v );
537 v3_muls( rb->w, 1.0f/(1.0f+k_rb_delta*k_damp_angular), rb->w );
538 }
539
540
541 /*
542 * -----------------------------------------------------------------------------
543 * Boolean shape overlap functions
544 * -----------------------------------------------------------------------------
545 */
546
547 /*
548 * Project AABB, and triangle interval onto axis to check if they overlap
549 */
550 VG_STATIC int rb_box_triangle_interval( v3f extent, v3f axis, v3f tri[3] )
551 {
552 float
553
554 r = extent[0] * fabsf(axis[0]) +
555 extent[1] * fabsf(axis[1]) +
556 extent[2] * fabsf(axis[2]),
557
558 p0 = v3_dot( axis, tri[0] ),
559 p1 = v3_dot( axis, tri[1] ),
560 p2 = v3_dot( axis, tri[2] ),
561
562 e = vg_maxf(-vg_maxf(p0,vg_maxf(p1,p2)), vg_minf(p0,vg_minf(p1,p2)));
563
564 if( e > r ) return 0;
565 else return 1;
566 }
567
568 /*
569 * Seperating axis test box vs triangle
570 */
571 VG_STATIC int rb_box_triangle_sat( rigidbody *rba, v3f tri_src[3] )
572 {
573 v3f tri[3];
574
575 v3f extent, c;
576 v3_sub( rba->bbx[1], rba->bbx[0], extent );
577 v3_muls( extent, 0.5f, extent );
578 v3_add( rba->bbx[0], extent, c );
579
580 for( int i=0; i<3; i++ )
581 {
582 m4x3_mulv( rba->to_local, tri_src[i], tri[i] );
583 v3_sub( tri[i], c, tri[i] );
584 }
585
586 /* u0, u1, u2 */
587 if(!rb_box_triangle_interval( extent, (v3f){1.0f,0.0f,0.0f}, tri )) return 0;
588 if(!rb_box_triangle_interval( extent, (v3f){0.0f,1.0f,0.0f}, tri )) return 0;
589 if(!rb_box_triangle_interval( extent, (v3f){0.0f,0.0f,1.0f}, tri )) return 0;
590
591 v3f v0,v1,v2,n, e0,e1,e2;
592 v3_sub( tri[1], tri[0], v0 );
593 v3_sub( tri[2], tri[0], v1 );
594 v3_sub( tri[2], tri[1], v2 );
595 v3_normalize( v0 );
596 v3_normalize( v1 );
597 v3_normalize( v2 );
598 v3_cross( v0, v1, n );
599 v3_cross( v0, n, e0 );
600 v3_cross( n, v1, e1 );
601 v3_cross( v2, n, e2 );
602
603 /* normal */
604 if(!rb_box_triangle_interval( extent, n, tri )) return 0;
605
606 v3f axis[9];
607 v3_cross( e0, (v3f){1.0f,0.0f,0.0f}, axis[0] );
608 v3_cross( e0, (v3f){0.0f,1.0f,0.0f}, axis[1] );
609 v3_cross( e0, (v3f){0.0f,0.0f,1.0f}, axis[2] );
610 v3_cross( e1, (v3f){1.0f,0.0f,0.0f}, axis[3] );
611 v3_cross( e1, (v3f){0.0f,1.0f,0.0f}, axis[4] );
612 v3_cross( e1, (v3f){0.0f,0.0f,1.0f}, axis[5] );
613 v3_cross( e2, (v3f){1.0f,0.0f,0.0f}, axis[6] );
614 v3_cross( e2, (v3f){0.0f,1.0f,0.0f}, axis[7] );
615 v3_cross( e2, (v3f){0.0f,0.0f,1.0f}, axis[8] );
616
617 for( int i=0; i<9; i++ )
618 if(!rb_box_triangle_interval( extent, axis[i], tri )) return 0;
619
620 return 1;
621 }
622
623 /*
624 * -----------------------------------------------------------------------------
625 * Manifold
626 * -----------------------------------------------------------------------------
627 */
628
629 VG_STATIC int rb_manifold_apply_filtered( rb_ct *man, int len )
630 {
631 int k = 0;
632
633 for( int i=0; i<len; i++ )
634 {
635 rb_ct *ct = &man[i];
636
637 if( ct->type == k_contact_type_disabled )
638 continue;
639
640 man[k ++] = man[i];
641 }
642
643 return k;
644 }
645
646 /*
647 * Merge two contacts if they are within radius(r) of eachother
648 */
649 VG_STATIC void rb_manifold_contact_weld( rb_ct *ci, rb_ct *cj, float r )
650 {
651 if( v3_dist2( ci->co, cj->co ) < r*r )
652 {
653 cj->type = k_contact_type_disabled;
654 ci->p = (ci->p + cj->p) * 0.5f;
655
656 v3_add( ci->co, cj->co, ci->co );
657 v3_muls( ci->co, 0.5f, ci->co );
658
659 v3f delta;
660 v3_sub( ci->rba->co, ci->co, delta );
661
662 float c0 = v3_dot( ci->n, delta ),
663 c1 = v3_dot( cj->n, delta );
664
665 if( c0 < 0.0f || c1 < 0.0f )
666 {
667 /* error */
668 ci->type = k_contact_type_disabled;
669 }
670 else
671 {
672 v3f n;
673 v3_muls( ci->n, c0, n );
674 v3_muladds( n, cj->n, c1, n );
675 v3_normalize( n );
676 v3_copy( n, ci->n );
677 }
678 }
679 }
680
681 /*
682 *
683 */
684 VG_STATIC void rb_manifold_filter_joint_edges( rb_ct *man, int len, float r )
685 {
686 for( int i=0; i<len-1; i++ )
687 {
688 rb_ct *ci = &man[i];
689 if( ci->type != k_contact_type_edge )
690 continue;
691
692 for( int j=i+1; j<len; j++ )
693 {
694 rb_ct *cj = &man[j];
695 if( cj->type != k_contact_type_edge )
696 continue;
697
698 rb_manifold_contact_weld( ci, cj, r );
699 }
700 }
701 }
702
703 /*
704 * Resolve overlapping pairs
705 *
706 * TODO: Remove?
707 */
708 VG_STATIC void rb_manifold_filter_pairs( rb_ct *man, int len, float r )
709 {
710 for( int i=0; i<len-1; i++ )
711 {
712 rb_ct *ci = &man[i];
713 int similar = 0;
714
715 if( ci->type == k_contact_type_disabled ) continue;
716
717 for( int j=i+1; j<len; j++ )
718 {
719 rb_ct *cj = &man[j];
720
721 if( cj->type == k_contact_type_disabled ) continue;
722
723 if( v3_dist2( ci->co, cj->co ) < r*r )
724 {
725 cj->type = k_contact_type_disabled;
726 v3_add( cj->n, ci->n, ci->n );
727 ci->p += cj->p;
728 similar ++;
729 }
730 }
731
732 if( similar )
733 {
734 float n = 1.0f/((float)similar+1.0f);
735 v3_muls( ci->n, n, ci->n );
736 ci->p *= n;
737
738 if( v3_length2(ci->n) < 0.1f*0.1f )
739 ci->type = k_contact_type_disabled;
740 else
741 v3_normalize( ci->n );
742 }
743 }
744 }
745
746 /*
747 * Remove contacts that are facing away from A
748 */
749 VG_STATIC void rb_manifold_filter_backface( rb_ct *man, int len )
750 {
751 for( int i=0; i<len; i++ )
752 {
753 rb_ct *ct = &man[i];
754 if( ct->type == k_contact_type_disabled )
755 continue;
756
757 v3f delta;
758 v3_sub( ct->co, ct->rba->co, delta );
759
760 if( v3_dot( delta, ct->n ) > -0.001f )
761 ct->type = k_contact_type_disabled;
762 }
763 }
764
765 /*
766 * Filter out duplicate coplanar results. Good for spheres.
767 */
768 VG_STATIC void rb_manifold_filter_coplanar( rb_ct *man, int len, float w )
769 {
770 for( int i=0; i<len; i++ )
771 {
772 rb_ct *ci = &man[i];
773 if( ci->type == k_contact_type_disabled ||
774 ci->type == k_contact_type_edge )
775 continue;
776
777 float d1 = v3_dot( ci->co, ci->n );
778
779 for( int j=0; j<len; j++ )
780 {
781 if( j == i )
782 continue;
783
784 rb_ct *cj = &man[j];
785 if( cj->type == k_contact_type_disabled )
786 continue;
787
788 float d2 = v3_dot( cj->co, ci->n ),
789 d = d2-d1;
790
791 if( fabsf( d ) <= w )
792 {
793 cj->type = k_contact_type_disabled;
794 }
795 }
796 }
797 }
798
799 /*
800 * -----------------------------------------------------------------------------
801 * Collision matrix
802 * -----------------------------------------------------------------------------
803 */
804
805 /*
806 * Contact generators
807 *
808 * These do not automatically allocate contacts, an appropriately sized
809 * buffer must be supplied. The function returns the size of the manifold
810 * which was generated.
811 *
812 * The values set on the contacts are: n, co, p, rba, rbb
813 */
814
815 /*
816 * By collecting the minimum(time) and maximum(time) pairs of points, we
817 * build a reduced and stable exact manifold.
818 *
819 * tx: time at point
820 * rx: minimum distance of these points
821 * dx: the delta between the two points
822 *
823 * pairs will only ammend these if they are creating a collision
824 */
825 typedef struct capsule_manifold capsule_manifold;
826 struct capsule_manifold
827 {
828 float t0, t1;
829 float r0, r1;
830 v3f d0, d1;
831 };
832
833 /*
834 * Expand a line manifold with a new pair. t value is the time along segment
835 * on the oriented object which created this pair.
836 */
837 VG_STATIC void rb_capsule_manifold( v3f pa, v3f pb, float t, float r,
838 capsule_manifold *manifold )
839 {
840 v3f delta;
841 v3_sub( pa, pb, delta );
842
843 if( v3_length2(delta) < r*r )
844 {
845 if( t < manifold->t0 )
846 {
847 v3_copy( delta, manifold->d0 );
848 manifold->t0 = t;
849 manifold->r0 = r;
850 }
851
852 if( t > manifold->t1 )
853 {
854 v3_copy( delta, manifold->d1 );
855 manifold->t1 = t;
856 manifold->r1 = r;
857 }
858 }
859 }
860
861 VG_STATIC void rb_capsule_manifold_init( capsule_manifold *manifold )
862 {
863 manifold->t0 = INFINITY;
864 manifold->t1 = -INFINITY;
865 }
866
867 __attribute__ ((deprecated))
868 VG_STATIC int rb_capsule_manifold_done( rigidbody *rba, rigidbody *rbb,
869 capsule_manifold *manifold, rb_ct *buf )
870 {
871 float h = rba->inf.capsule.height,
872 ra = rba->inf.capsule.radius;
873
874 v3f p0, p1;
875 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
876 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
877
878 int count = 0;
879 if( manifold->t0 <= 1.0f )
880 {
881 rb_ct *ct = buf;
882
883 v3f pa;
884 v3_muls( p0, 1.0f-manifold->t0, pa );
885 v3_muladds( pa, p1, manifold->t0, pa );
886
887 float d = v3_length( manifold->d0 );
888 v3_muls( manifold->d0, 1.0f/d, ct->n );
889 v3_muladds( pa, ct->n, -ra, ct->co );
890
891 ct->p = manifold->r0 - d;
892 ct->rba = rba;
893 ct->rbb = rbb;
894 ct->type = k_contact_type_default;
895
896 count ++;
897 }
898
899 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
900 {
901 rb_ct *ct = buf+count;
902
903 v3f pa;
904 v3_muls( p0, 1.0f-manifold->t1, pa );
905 v3_muladds( pa, p1, manifold->t1, pa );
906
907 float d = v3_length( manifold->d1 );
908 v3_muls( manifold->d1, 1.0f/d, ct->n );
909 v3_muladds( pa, ct->n, -ra, ct->co );
910
911 ct->p = manifold->r1 - d;
912 ct->rba = rba;
913 ct->rbb = rbb;
914 ct->type = k_contact_type_default;
915
916 count ++;
917 }
918
919 /*
920 * Debugging
921 */
922
923 if( count == 2 )
924 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
925
926 return count;
927 }
928
929 VG_STATIC int rb_capsule__manifold_done( m4x3f mtx, rb_capsule *c,
930 capsule_manifold *manifold,
931 rb_ct *buf )
932 {
933 v3f p0, p1;
934 v3_muladds( mtx[3], mtx[1], -c->height*0.5f+c->radius, p0 );
935 v3_muladds( mtx[3], mtx[1], c->height*0.5f-c->radius, p1 );
936
937 int count = 0;
938 if( manifold->t0 <= 1.0f )
939 {
940 rb_ct *ct = buf;
941
942 v3f pa;
943 v3_muls( p0, 1.0f-manifold->t0, pa );
944 v3_muladds( pa, p1, manifold->t0, pa );
945
946 float d = v3_length( manifold->d0 );
947 v3_muls( manifold->d0, 1.0f/d, ct->n );
948 v3_muladds( pa, ct->n, -c->radius, ct->co );
949
950 ct->p = manifold->r0 - d;
951 ct->type = k_contact_type_default;
952 count ++;
953 }
954
955 if( (manifold->t1 >= 0.0f) && (manifold->t0 != manifold->t1) )
956 {
957 rb_ct *ct = buf+count;
958
959 v3f pa;
960 v3_muls( p0, 1.0f-manifold->t1, pa );
961 v3_muladds( pa, p1, manifold->t1, pa );
962
963 float d = v3_length( manifold->d1 );
964 v3_muls( manifold->d1, 1.0f/d, ct->n );
965 v3_muladds( pa, ct->n, -c->radius, ct->co );
966
967 ct->p = manifold->r1 - d;
968 ct->type = k_contact_type_default;
969
970 count ++;
971 }
972
973 /*
974 * Debugging
975 */
976
977 if( count == 2 )
978 vg_line( buf[0].co, buf[1].co, 0xff0000ff );
979
980 return count;
981 }
982
983 VG_STATIC int rb_capsule_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
984 {
985 float h = rba->inf.capsule.height,
986 ra = rba->inf.capsule.radius,
987 rb = rbb->inf.sphere.radius;
988
989 v3f p0, p1;
990 v3_muladds( rba->co, rba->up, -h*0.5f+ra, p0 );
991 v3_muladds( rba->co, rba->up, h*0.5f-ra, p1 );
992
993 v3f c, delta;
994 closest_point_segment( p0, p1, rbb->co, c );
995 v3_sub( c, rbb->co, delta );
996
997 float d2 = v3_length2(delta),
998 r = ra + rb;
999
1000 if( d2 < r*r )
1001 {
1002 float d = sqrtf(d2);
1003
1004 rb_ct *ct = buf;
1005 v3_muls( delta, 1.0f/d, ct->n );
1006 ct->p = r-d;
1007
1008 v3f p0, p1;
1009 v3_muladds( c, ct->n, -ra, p0 );
1010 v3_muladds( rbb->co, ct->n, rb, p1 );
1011 v3_add( p0, p1, ct->co );
1012 v3_muls( ct->co, 0.5f, ct->co );
1013
1014 ct->rba = rba;
1015 ct->rbb = rbb;
1016 ct->type = k_contact_type_default;
1017
1018 return 1;
1019 }
1020
1021 return 0;
1022 }
1023
1024 VG_STATIC int rb_capsule_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1025 {
1026 if( !box_overlap( rba->bbx_world, rbb->bbx_world ) )
1027 return 0;
1028
1029 float ha = rba->inf.capsule.height,
1030 hb = rbb->inf.capsule.height,
1031 ra = rba->inf.capsule.radius,
1032 rb = rbb->inf.capsule.radius,
1033 r = ra+rb;
1034
1035 v3f p0, p1, p2, p3;
1036 v3_muladds( rba->co, rba->up, -ha*0.5f+ra, p0 );
1037 v3_muladds( rba->co, rba->up, ha*0.5f-ra, p1 );
1038 v3_muladds( rbb->co, rbb->up, -hb*0.5f+rb, p2 );
1039 v3_muladds( rbb->co, rbb->up, hb*0.5f-rb, p3 );
1040
1041 capsule_manifold manifold;
1042 rb_capsule_manifold_init( &manifold );
1043
1044 v3f pa, pb;
1045 float ta, tb;
1046 closest_segment_segment( p0, p1, p2, p3, &ta, &tb, pa, pb );
1047 rb_capsule_manifold( pa, pb, ta, r, &manifold );
1048
1049 ta = closest_point_segment( p0, p1, p2, pa );
1050 tb = closest_point_segment( p0, p1, p3, pb );
1051 rb_capsule_manifold( pa, p2, ta, r, &manifold );
1052 rb_capsule_manifold( pb, p3, tb, r, &manifold );
1053
1054 closest_point_segment( p2, p3, p0, pa );
1055 closest_point_segment( p2, p3, p1, pb );
1056 rb_capsule_manifold( p0, pa, 0.0f, r, &manifold );
1057 rb_capsule_manifold( p1, pb, 1.0f, r, &manifold );
1058
1059 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1060 }
1061
1062 /*
1063 * Generates up to two contacts; optimised for the most stable manifold
1064 */
1065 VG_STATIC int rb_capsule_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1066 {
1067 float h = rba->inf.capsule.height,
1068 r = rba->inf.capsule.radius;
1069
1070 /*
1071 * Solving this in symetric local space of the cube saves us some time and a
1072 * couple branches when it comes to the quad stage.
1073 */
1074 v3f centroid;
1075 v3_add( rbb->bbx[0], rbb->bbx[1], centroid );
1076 v3_muls( centroid, 0.5f, centroid );
1077
1078 boxf bbx;
1079 v3_sub( rbb->bbx[0], centroid, bbx[0] );
1080 v3_sub( rbb->bbx[1], centroid, bbx[1] );
1081
1082 v3f pc, p0w, p1w, p0, p1;
1083 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
1084 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
1085
1086 m4x3_mulv( rbb->to_local, p0w, p0 );
1087 m4x3_mulv( rbb->to_local, p1w, p1 );
1088 v3_sub( p0, centroid, p0 );
1089 v3_sub( p1, centroid, p1 );
1090 v3_add( p0, p1, pc );
1091 v3_muls( pc, 0.5f, pc );
1092
1093 /*
1094 * Finding an appropriate quad to collide lines with
1095 */
1096 v3f region;
1097 v3_div( pc, bbx[1], region );
1098
1099 v3f quad[4];
1100 if( (fabsf(region[0]) > fabsf(region[1])) &&
1101 (fabsf(region[0]) > fabsf(region[2])) )
1102 {
1103 float px = vg_signf(region[0]) * bbx[1][0];
1104 v3_copy( (v3f){ px, bbx[0][1], bbx[0][2] }, quad[0] );
1105 v3_copy( (v3f){ px, bbx[1][1], bbx[0][2] }, quad[1] );
1106 v3_copy( (v3f){ px, bbx[1][1], bbx[1][2] }, quad[2] );
1107 v3_copy( (v3f){ px, bbx[0][1], bbx[1][2] }, quad[3] );
1108 }
1109 else if( fabsf(region[1]) > fabsf(region[2]) )
1110 {
1111 float py = vg_signf(region[1]) * bbx[1][1];
1112 v3_copy( (v3f){ bbx[0][0], py, bbx[0][2] }, quad[0] );
1113 v3_copy( (v3f){ bbx[1][0], py, bbx[0][2] }, quad[1] );
1114 v3_copy( (v3f){ bbx[1][0], py, bbx[1][2] }, quad[2] );
1115 v3_copy( (v3f){ bbx[0][0], py, bbx[1][2] }, quad[3] );
1116 }
1117 else
1118 {
1119 float pz = vg_signf(region[2]) * bbx[1][2];
1120 v3_copy( (v3f){ bbx[0][0], bbx[0][1], pz }, quad[0] );
1121 v3_copy( (v3f){ bbx[1][0], bbx[0][1], pz }, quad[1] );
1122 v3_copy( (v3f){ bbx[1][0], bbx[1][1], pz }, quad[2] );
1123 v3_copy( (v3f){ bbx[0][0], bbx[1][1], pz }, quad[3] );
1124 }
1125
1126 capsule_manifold manifold;
1127 rb_capsule_manifold_init( &manifold );
1128
1129 v3f c0, c1;
1130 closest_point_aabb( p0, bbx, c0 );
1131 closest_point_aabb( p1, bbx, c1 );
1132
1133 v3f d0, d1, da;
1134 v3_sub( c0, p0, d0 );
1135 v3_sub( c1, p1, d1 );
1136 v3_sub( p1, p0, da );
1137
1138 v3_normalize(d0);
1139 v3_normalize(d1);
1140 v3_normalize(da);
1141
1142 if( v3_dot( da, d0 ) <= 0.01f )
1143 rb_capsule_manifold( p0, c0, 0.0f, r, &manifold );
1144
1145 if( v3_dot( da, d1 ) >= -0.01f )
1146 rb_capsule_manifold( p1, c1, 1.0f, r, &manifold );
1147
1148 for( int i=0; i<4; i++ )
1149 {
1150 int i0 = i,
1151 i1 = (i+1)%4;
1152
1153 v3f ca, cb;
1154 float ta, tb;
1155 closest_segment_segment( p0, p1, quad[i0], quad[i1], &ta, &tb, ca, cb );
1156 rb_capsule_manifold( ca, cb, ta, r, &manifold );
1157 }
1158
1159 /*
1160 * Create final contacts based on line manifold
1161 */
1162 m3x3_mulv( rbb->to_world, manifold.d0, manifold.d0 );
1163 m3x3_mulv( rbb->to_world, manifold.d1, manifold.d1 );
1164
1165 /*
1166 * Debugging
1167 */
1168
1169 #if 0
1170 for( int i=0; i<4; i++ )
1171 {
1172 v3f q0, q1;
1173 int i0 = i,
1174 i1 = (i+1)%4;
1175
1176 v3_add( quad[i0], centroid, q0 );
1177 v3_add( quad[i1], centroid, q1 );
1178
1179 m4x3_mulv( rbb->to_world, q0, q0 );
1180 m4x3_mulv( rbb->to_world, q1, q1 );
1181
1182 vg_line( q0, q1, 0xffffffff );
1183 }
1184 #endif
1185
1186 return rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1187 }
1188
1189 VG_STATIC int rb_sphere_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1190 {
1191 v3f co, delta;
1192
1193 closest_point_obb( rba->co, rbb->bbx, rbb->to_world, rbb->to_local, co );
1194 v3_sub( rba->co, co, delta );
1195
1196 float d2 = v3_length2(delta),
1197 r = rba->inf.sphere.radius;
1198
1199 if( d2 <= r*r )
1200 {
1201 float d;
1202
1203 rb_ct *ct = buf;
1204 if( d2 <= 0.0001f )
1205 {
1206 v3_sub( rba->co, rbb->co, delta );
1207
1208 /*
1209 * some extra testing is required to find the best axis to push the
1210 * object back outside the box. Since there isnt a clear seperating
1211 * vector already, especially on really high aspect boxes.
1212 */
1213 float lx = v3_dot( rbb->right, delta ),
1214 ly = v3_dot( rbb->up, delta ),
1215 lz = v3_dot( rbb->forward, delta ),
1216 px = rbb->bbx[1][0] - fabsf(lx),
1217 py = rbb->bbx[1][1] - fabsf(ly),
1218 pz = rbb->bbx[1][2] - fabsf(lz);
1219
1220 if( px < py && px < pz )
1221 v3_muls( rbb->right, vg_signf(lx), ct->n );
1222 else if( py < pz )
1223 v3_muls( rbb->up, vg_signf(ly), ct->n );
1224 else
1225 v3_muls( rbb->forward, vg_signf(lz), ct->n );
1226
1227 v3_muladds( rba->co, ct->n, -r, ct->co );
1228 ct->p = r;
1229 }
1230 else
1231 {
1232 d = sqrtf(d2);
1233 v3_muls( delta, 1.0f/d, ct->n );
1234 ct->p = r-d;
1235 v3_copy( co, ct->co );
1236 }
1237
1238 ct->rba = rba;
1239 ct->rbb = rbb;
1240 ct->type = k_contact_type_default;
1241 return 1;
1242 }
1243
1244 return 0;
1245 }
1246
1247 VG_STATIC int rb_sphere_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1248 {
1249 v3f delta;
1250 v3_sub( rba->co, rbb->co, delta );
1251
1252 float d2 = v3_length2(delta),
1253 r = rba->inf.sphere.radius + rbb->inf.sphere.radius;
1254
1255 if( d2 < r*r )
1256 {
1257 float d = sqrtf(d2);
1258
1259 rb_ct *ct = buf;
1260 v3_muls( delta, 1.0f/d, ct->n );
1261
1262 v3f p0, p1;
1263 v3_muladds( rba->co, ct->n,-rba->inf.sphere.radius, p0 );
1264 v3_muladds( rbb->co, ct->n, rbb->inf.sphere.radius, p1 );
1265 v3_add( p0, p1, ct->co );
1266 v3_muls( ct->co, 0.5f, ct->co );
1267 ct->type = k_contact_type_default;
1268 ct->p = r-d;
1269 ct->rba = rba;
1270 ct->rbb = rbb;
1271 return 1;
1272 }
1273
1274 return 0;
1275 }
1276
1277 //#define RIGIDBODY_DYNAMIC_MESH_EDGES
1278
1279 VG_STATIC int rb_sphere_triangle( rigidbody *rba, rigidbody *rbb,
1280 v3f tri[3], rb_ct *buf )
1281 {
1282 v3f delta, co;
1283
1284 #ifdef RIGIDBODY_DYNAMIC_MESH_EDGES
1285 closest_on_triangle_1( rba->co, tri, co );
1286 #else
1287 enum contact_type type = closest_on_triangle_1( rba->co, tri, co );
1288 #endif
1289
1290 v3_sub( rba->co, co, delta );
1291
1292 float d2 = v3_length2( delta ),
1293 r = rba->inf.sphere.radius;
1294
1295 if( d2 < r*r )
1296 {
1297 rb_ct *ct = buf;
1298
1299 v3f ab, ac, tn;
1300 v3_sub( tri[2], tri[0], ab );
1301 v3_sub( tri[1], tri[0], ac );
1302 v3_cross( ac, ab, tn );
1303 v3_copy( tn, ct->n );
1304
1305 if( v3_length2( ct->n ) <= 0.00001f )
1306 {
1307 vg_error( "Zero area triangle!\n" );
1308 return 0;
1309 }
1310
1311 v3_normalize( ct->n );
1312
1313 float d = sqrtf(d2);
1314
1315 v3_copy( co, ct->co );
1316 ct->type = type;
1317 ct->p = r-d;
1318 ct->rba = rba;
1319 ct->rbb = rbb;
1320 return 1;
1321 }
1322
1323 return 0;
1324 }
1325
1326
1327 VG_STATIC void rb_debug_sharp_scene_edges( rigidbody *rbb, float sharp_ang,
1328 boxf box, u32 colour )
1329 {
1330 sharp_ang = cosf( sharp_ang );
1331
1332 scene *sc = rbb->inf.scene.bh_scene->user;
1333 vg_line_boxf( box, 0xff00ff00 );
1334
1335 bh_iter it;
1336 bh_iter_init( 0, &it );
1337 int idx;
1338
1339 while( bh_next( rbb->inf.scene.bh_scene, &it, box, &idx ) )
1340 {
1341 u32 *ptri = &sc->arrindices[ idx*3 ];
1342 v3f tri[3];
1343
1344 for( int j=0; j<3; j++ )
1345 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1346
1347 for( int j=0; j<3; j++ )
1348 {
1349 #if 0
1350 v3f edir;
1351 v3_sub( tri[(j+1)%3], tri[j], edir );
1352
1353 if( v3_dot( edir, (v3f){ 0.5184758473652127f,
1354 0.2073903389460850f,
1355 -0.8295613557843402f } ) < 0.0f )
1356 continue;
1357 #endif
1358
1359 bh_iter jt;
1360 bh_iter_init( 0, &jt );
1361
1362 boxf region;
1363 float const k_r = 0.02f;
1364 v3_add( (v3f){ k_r, k_r, k_r }, tri[j], region[1] );
1365 v3_add( (v3f){ -k_r, -k_r, -k_r }, tri[j], region[0] );
1366
1367 int jdx;
1368 while( bh_next( rbb->inf.scene.bh_scene, &jt, region, &jdx ) )
1369 {
1370 if( idx <= jdx )
1371 continue;
1372
1373 u32 *ptrj = &sc->arrindices[ jdx*3 ];
1374 v3f trj[3];
1375
1376 for( int k=0; k<3; k++ )
1377 v3_copy( sc->arrvertices[ptrj[k]].co, trj[k] );
1378
1379 for( int k=0; k<3; k++ )
1380 {
1381 if( v3_dist2( tri[j], trj[k] ) <= k_r*k_r )
1382 {
1383 int jp1 = (j+1)%3,
1384 jp2 = (j+2)%3,
1385 km1 = (k+3-1)%3,
1386 km2 = (k+3-2)%3;
1387
1388 if( v3_dist2( tri[jp1], trj[km1] ) <= k_r*k_r )
1389 {
1390 v3f b0, b1, b2;
1391 v3_sub( tri[jp1], tri[j], b0 );
1392 v3_sub( tri[jp2], tri[j], b1 );
1393 v3_sub( trj[km2], tri[j], b2 );
1394
1395 v3f cx0, cx1;
1396 v3_cross( b0, b1, cx0 );
1397 v3_cross( b2, b0, cx1 );
1398
1399 float polarity = v3_dot( cx0, b2 );
1400
1401 if( polarity < 0.0f )
1402 {
1403 #if 0
1404 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1405 float ang = v3_dot(cx0,cx1) /
1406 (v3_length(cx0)*v3_length(cx1));
1407 if( ang < sharp_ang )
1408 {
1409 vg_line( tri[j], tri[jp1], 0xff00ff00 );
1410 }
1411 #endif
1412 }
1413 }
1414 }
1415 }
1416 }
1417 }
1418 }
1419 }
1420
1421 VG_STATIC int rb_sphere_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1422 {
1423 scene *sc = rbb->inf.scene.bh_scene->user;
1424
1425 bh_iter it;
1426 bh_iter_init( 0, &it );
1427 int idx;
1428
1429 int count = 0;
1430
1431 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1432 {
1433 u32 *ptri = &sc->arrindices[ idx*3 ];
1434 v3f tri[3];
1435
1436 for( int j=0; j<3; j++ )
1437 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1438
1439 buf[ count ].element_id = ptri[0];
1440
1441 vg_line( tri[0],tri[1],0x70ff6000 );
1442 vg_line( tri[1],tri[2],0x70ff6000 );
1443 vg_line( tri[2],tri[0],0x70ff6000 );
1444
1445 int contact = rb_sphere_triangle( rba, rbb, tri, buf+count );
1446 count += contact;
1447
1448 if( count == 16 )
1449 {
1450 vg_warn( "Exceeding sphere_vs_scene capacity. Geometry too dense!\n" );
1451 return count;
1452 }
1453 }
1454
1455 return count;
1456 }
1457
1458 VG_STATIC int rb_box_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1459 {
1460 scene *sc = rbb->inf.scene.bh_scene->user;
1461
1462 v3f tri[3];
1463
1464 bh_iter it;
1465 bh_iter_init( 0, &it );
1466 int idx;
1467
1468 int count = 0;
1469
1470 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1471 {
1472 u32 *ptri = &sc->arrindices[ idx*3 ];
1473
1474 for( int j=0; j<3; j++ )
1475 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1476
1477 if( rb_box_triangle_sat( rba, tri ) )
1478 {
1479 vg_line(tri[0],tri[1],0xff50ff00 );
1480 vg_line(tri[1],tri[2],0xff50ff00 );
1481 vg_line(tri[2],tri[0],0xff50ff00 );
1482 }
1483 else
1484 {
1485 vg_line(tri[0],tri[1],0xff0000ff );
1486 vg_line(tri[1],tri[2],0xff0000ff );
1487 vg_line(tri[2],tri[0],0xff0000ff );
1488
1489 continue;
1490 }
1491
1492 v3f v0,v1,n;
1493 v3_sub( tri[1], tri[0], v0 );
1494 v3_sub( tri[2], tri[0], v1 );
1495 v3_cross( v0, v1, n );
1496 v3_normalize( n );
1497
1498 /* find best feature */
1499 float best = v3_dot( rba->right, n );
1500 int axis = 0;
1501
1502 float cy = v3_dot( rba->up, n );
1503 if( fabsf(cy) > fabsf(best) )
1504 {
1505 best = cy;
1506 axis = 1;
1507 }
1508
1509 float cz = -v3_dot( rba->forward, n );
1510 if( fabsf(cz) > fabsf(best) )
1511 {
1512 best = cz;
1513 axis = 2;
1514 }
1515
1516 v3f manifold[4];
1517
1518 if( axis == 0 )
1519 {
1520 float px = best > 0.0f? rba->bbx[0][0]: rba->bbx[1][0];
1521 manifold[0][0] = px;
1522 manifold[0][1] = rba->bbx[0][1];
1523 manifold[0][2] = rba->bbx[0][2];
1524 manifold[1][0] = px;
1525 manifold[1][1] = rba->bbx[1][1];
1526 manifold[1][2] = rba->bbx[0][2];
1527 manifold[2][0] = px;
1528 manifold[2][1] = rba->bbx[1][1];
1529 manifold[2][2] = rba->bbx[1][2];
1530 manifold[3][0] = px;
1531 manifold[3][1] = rba->bbx[0][1];
1532 manifold[3][2] = rba->bbx[1][2];
1533 }
1534 else if( axis == 1 )
1535 {
1536 float py = best > 0.0f? rba->bbx[0][1]: rba->bbx[1][1];
1537 manifold[0][0] = rba->bbx[0][0];
1538 manifold[0][1] = py;
1539 manifold[0][2] = rba->bbx[0][2];
1540 manifold[1][0] = rba->bbx[1][0];
1541 manifold[1][1] = py;
1542 manifold[1][2] = rba->bbx[0][2];
1543 manifold[2][0] = rba->bbx[1][0];
1544 manifold[2][1] = py;
1545 manifold[2][2] = rba->bbx[1][2];
1546 manifold[3][0] = rba->bbx[0][0];
1547 manifold[3][1] = py;
1548 manifold[3][2] = rba->bbx[1][2];
1549 }
1550 else
1551 {
1552 float pz = best > 0.0f? rba->bbx[0][2]: rba->bbx[1][2];
1553 manifold[0][0] = rba->bbx[0][0];
1554 manifold[0][1] = rba->bbx[0][1];
1555 manifold[0][2] = pz;
1556 manifold[1][0] = rba->bbx[1][0];
1557 manifold[1][1] = rba->bbx[0][1];
1558 manifold[1][2] = pz;
1559 manifold[2][0] = rba->bbx[1][0];
1560 manifold[2][1] = rba->bbx[1][1];
1561 manifold[2][2] = pz;
1562 manifold[3][0] = rba->bbx[0][0];
1563 manifold[3][1] = rba->bbx[1][1];
1564 manifold[3][2] = pz;
1565 }
1566
1567 for( int j=0; j<4; j++ )
1568 m4x3_mulv( rba->to_world, manifold[j], manifold[j] );
1569
1570 vg_line( manifold[0], manifold[1], 0xffffffff );
1571 vg_line( manifold[1], manifold[2], 0xffffffff );
1572 vg_line( manifold[2], manifold[3], 0xffffffff );
1573 vg_line( manifold[3], manifold[0], 0xffffffff );
1574
1575 for( int j=0; j<4; j++ )
1576 {
1577 rb_ct *ct = buf+count;
1578
1579 v3_copy( manifold[j], ct->co );
1580 v3_copy( n, ct->n );
1581
1582 float l0 = v3_dot( tri[0], n ),
1583 l1 = v3_dot( manifold[j], n );
1584
1585 ct->p = (l0-l1)*0.5f;
1586 if( ct->p < 0.0f )
1587 continue;
1588
1589 ct->type = k_contact_type_default;
1590 ct->rba = rba;
1591 ct->rbb = rbb;
1592 count ++;
1593
1594 if( count >= 12 )
1595 return count;
1596 }
1597 }
1598 return count;
1599 }
1600
1601 VG_STATIC int rb_capsule__triangle( m4x3f mtxA, rb_capsule *c,
1602 v3f tri[3], rb_ct *buf )
1603 {
1604 v3f pc, p0w, p1w;
1605 v3_muladds( mtxA[3], mtxA[1], -c->height*0.5f+c->radius, p0w );
1606 v3_muladds( mtxA[3], mtxA[1], c->height*0.5f-c->radius, p1w );
1607
1608 capsule_manifold manifold;
1609 rb_capsule_manifold_init( &manifold );
1610
1611 v3f c0, c1;
1612 closest_on_triangle_1( p0w, tri, c0 );
1613 closest_on_triangle_1( p1w, tri, c1 );
1614
1615 v3f d0, d1, da;
1616 v3_sub( c0, p0w, d0 );
1617 v3_sub( c1, p1w, d1 );
1618 v3_sub( p1w, p0w, da );
1619
1620 v3_normalize(d0);
1621 v3_normalize(d1);
1622 v3_normalize(da);
1623
1624 if( v3_dot( da, d0 ) <= 0.01f )
1625 rb_capsule_manifold( p0w, c0, 0.0f, c->radius, &manifold );
1626
1627 if( v3_dot( da, d1 ) >= -0.01f )
1628 rb_capsule_manifold( p1w, c1, 1.0f, c->radius, &manifold );
1629
1630 for( int i=0; i<3; i++ )
1631 {
1632 int i0 = i,
1633 i1 = (i+1)%3;
1634
1635 v3f ca, cb;
1636 float ta, tb;
1637 closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb );
1638 rb_capsule_manifold( ca, cb, ta, c->radius, &manifold );
1639 }
1640
1641 v3f v0, v1, n;
1642 v3_sub( tri[1], tri[0], v0 );
1643 v3_sub( tri[2], tri[0], v1 );
1644 v3_cross( v0, v1, n );
1645 v3_normalize( n );
1646
1647 int count = rb_capsule__manifold_done( mtxA, c, &manifold, buf );
1648 for( int i=0; i<count; i++ )
1649 v3_copy( n, buf[i].n );
1650
1651 return count;
1652 }
1653
1654 /*
1655 * Generates up to two contacts; optimised for the most stable manifold
1656 */
1657 __attribute__ ((deprecated))
1658 VG_STATIC int rb_capsule_triangle( rigidbody *rba, rigidbody *rbb,
1659 v3f tri[3], rb_ct *buf )
1660 {
1661 float h = rba->inf.capsule.height,
1662 r = rba->inf.capsule.radius;
1663
1664 v3f pc, p0w, p1w;
1665 v3_muladds( rba->co, rba->up, -h*0.5f+r, p0w );
1666 v3_muladds( rba->co, rba->up, h*0.5f-r, p1w );
1667
1668 capsule_manifold manifold;
1669 rb_capsule_manifold_init( &manifold );
1670
1671 v3f c0, c1;
1672 closest_on_triangle_1( p0w, tri, c0 );
1673 closest_on_triangle_1( p1w, tri, c1 );
1674
1675 v3f d0, d1, da;
1676 v3_sub( c0, p0w, d0 );
1677 v3_sub( c1, p1w, d1 );
1678 v3_sub( p1w, p0w, da );
1679
1680 v3_normalize(d0);
1681 v3_normalize(d1);
1682 v3_normalize(da);
1683
1684 if( v3_dot( da, d0 ) <= 0.01f )
1685 rb_capsule_manifold( p0w, c0, 0.0f, r, &manifold );
1686
1687 if( v3_dot( da, d1 ) >= -0.01f )
1688 rb_capsule_manifold( p1w, c1, 1.0f, r, &manifold );
1689
1690 for( int i=0; i<3; i++ )
1691 {
1692 int i0 = i,
1693 i1 = (i+1)%3;
1694
1695 v3f ca, cb;
1696 float ta, tb;
1697 closest_segment_segment( p0w, p1w, tri[i0], tri[i1], &ta, &tb, ca, cb );
1698 rb_capsule_manifold( ca, cb, ta, r, &manifold );
1699 }
1700
1701 v3f v0, v1, n;
1702 v3_sub( tri[1], tri[0], v0 );
1703 v3_sub( tri[2], tri[0], v1 );
1704 v3_cross( v0, v1, n );
1705 v3_normalize( n );
1706
1707 int count = rb_capsule_manifold_done( rba, rbb, &manifold, buf );
1708 for( int i=0; i<count; i++ )
1709 v3_copy( n, buf[i].n );
1710
1711 return count;
1712 }
1713
1714 /* mtxB is defined only for tradition; it is not used currently */
1715 VG_STATIC int rb_capsule__scene( m4x3f mtxA, rb_capsule *c,
1716 m4x3f mtxB, rb_scene *s,
1717 rb_ct *buf )
1718 {
1719 bh_iter it;
1720 bh_iter_init( 0, &it );
1721 int idx;
1722 int count = 0;
1723
1724 boxf bbx;
1725 v3_sub( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[0] );
1726 v3_add( mtxA[3], (v3f){ c->height, c->height, c->height }, bbx[1] );
1727
1728 scene *sc = s->bh_scene->user;
1729
1730 while( bh_next( s->bh_scene, &it, bbx, &idx ) )
1731 {
1732 u32 *ptri = &sc->arrindices[ idx*3 ];
1733 v3f tri[3];
1734
1735 for( int j=0; j<3; j++ )
1736 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1737
1738 buf[ count ].element_id = ptri[0];
1739
1740 int contact = rb_capsule__triangle( mtxA, c, tri, &buf[count] );
1741 count += contact;
1742
1743 if( count == 16 )
1744 {
1745 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1746 return count;
1747 }
1748 }
1749
1750 return count;
1751 }
1752
1753 __attribute__ ((deprecated))
1754 VG_STATIC int rb_capsule_scene( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1755 {
1756 scene *sc = rbb->inf.scene.bh_scene->user;
1757
1758 bh_iter it;
1759 bh_iter_init( 0, &it );
1760 int idx;
1761
1762 int count = 0;
1763
1764 while( bh_next( rbb->inf.scene.bh_scene, &it, rba->bbx_world, &idx ) )
1765 {
1766 u32 *ptri = &sc->arrindices[ idx*3 ];
1767 v3f tri[3];
1768
1769 for( int j=0; j<3; j++ )
1770 v3_copy( sc->arrvertices[ptri[j]].co, tri[j] );
1771
1772 buf[ count ].element_id = ptri[0];
1773
1774 #if 0
1775 vg_line( tri[0],tri[1],0x70ff6000 );
1776 vg_line( tri[1],tri[2],0x70ff6000 );
1777 vg_line( tri[2],tri[0],0x70ff6000 );
1778 #endif
1779
1780 int contact = rb_capsule_triangle( rba, rbb, tri, buf+count );
1781 count += contact;
1782
1783 if( count == 16 )
1784 {
1785 vg_warn("Exceeding capsule_vs_scene capacity. Geometry too dense!\n");
1786 return count;
1787 }
1788 }
1789
1790 return count;
1791 }
1792
1793 VG_STATIC int rb_scene_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1794 {
1795 return rb_capsule_scene( rbb, rba, buf );
1796 }
1797
1798 VG_STATIC int RB_MATRIX_ERROR( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1799 {
1800 #if 0
1801 vg_error( "Collision type is unimplemented between types %d and %d\n",
1802 rba->type, rbb->type );
1803 #endif
1804
1805 return 0;
1806 }
1807
1808 VG_STATIC int rb_sphere_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1809 {
1810 return rb_capsule_sphere( rbb, rba, buf );
1811 }
1812
1813 VG_STATIC int rb_box_capsule( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1814 {
1815 return rb_capsule_box( rbb, rba, buf );
1816 }
1817
1818 VG_STATIC int rb_box_sphere( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1819 {
1820 return rb_sphere_box( rbb, rba, buf );
1821 }
1822
1823 VG_STATIC int rb_scene_box( rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1824 {
1825 return rb_box_scene( rbb, rba, buf );
1826 }
1827
1828 VG_STATIC int (*rb_jump_table[4][4])( rigidbody *a, rigidbody *b, rb_ct *buf ) =
1829 {
1830 /* box */ /* Sphere */ /* Capsule */ /* Mesh */
1831 { RB_MATRIX_ERROR, rb_box_sphere, rb_box_capsule, rb_box_scene },
1832 { rb_sphere_box, rb_sphere_sphere, rb_sphere_capsule, rb_sphere_scene },
1833 { rb_capsule_box, rb_capsule_sphere, rb_capsule_capsule, rb_capsule_scene },
1834 { rb_scene_box, RB_MATRIX_ERROR, rb_scene_capsule, RB_MATRIX_ERROR }
1835 };
1836
1837 VG_STATIC int rb_collide( rigidbody *rba, rigidbody *rbb )
1838 {
1839 int (*collider_jump)(rigidbody *rba, rigidbody *rbb, rb_ct *buf )
1840 = rb_jump_table[rba->type][rbb->type];
1841
1842 /*
1843 * 12 is the maximum manifold size we can generate, so we are forced to abort
1844 * potentially checking any more.
1845 */
1846 if( rb_contact_count + 12 > vg_list_size(rb_contact_buffer) )
1847 {
1848 vg_warn( "Too many contacts made in global collider buffer (%d of %d\n)",
1849 rb_contact_count, vg_list_size(rb_contact_buffer) );
1850 return 0;
1851 }
1852
1853 /*
1854 * FUTURE: Replace this with a more dedicated broad phase pass
1855 */
1856 if( box_overlap( rba->bbx_world, rbb->bbx_world ) )
1857 {
1858 int count = collider_jump( rba, rbb, rb_contact_buffer+rb_contact_count);
1859 rb_contact_count += count;
1860 return count;
1861 }
1862 else
1863 return 0;
1864 }
1865
1866 /*
1867 * -----------------------------------------------------------------------------
1868 * Dynamics
1869 * -----------------------------------------------------------------------------
1870 */
1871
1872 VG_STATIC void rb_solver_reset(void)
1873 {
1874 rb_contact_count = 0;
1875 }
1876
1877 VG_STATIC rb_ct *rb_global_ct(void)
1878 {
1879 return rb_contact_buffer + rb_contact_count;
1880 }
1881
1882 VG_STATIC void rb_prepare_contact( rb_ct *ct )
1883 {
1884 ct->bias = -0.2f * k_rb_rate * vg_minf( 0.0f, -ct->p+k_penetration_slop );
1885 rb_tangent_basis( ct->n, ct->t[0], ct->t[1] );
1886
1887 #if 0
1888 ct->type = k_contact_type_default;
1889 #endif
1890 ct->norm_impulse = 0.0f;
1891 ct->tangent_impulse[0] = 0.0f;
1892 ct->tangent_impulse[1] = 0.0f;
1893 }
1894
1895 /*
1896 * Initializing things like tangent vectors
1897 */
1898 VG_STATIC void rb_presolve_contacts( rb_ct *buffer, int len )
1899 {
1900 for( int i=0; i<len; i++ )
1901 {
1902 rb_ct *ct = &buffer[i];
1903 rb_prepare_contact( ct );
1904
1905 v3f ra, rb, raCn, rbCn, raCt, rbCt;
1906 v3_sub( ct->co, ct->rba->co, ra );
1907 v3_sub( ct->co, ct->rbb->co, rb );
1908 v3_cross( ra, ct->n, raCn );
1909 v3_cross( rb, ct->n, rbCn );
1910
1911 /* orient inverse inertia tensors */
1912 v3f raCnI, rbCnI;
1913 m3x3_mulv( ct->rba->iIw, raCn, raCnI );
1914 m3x3_mulv( ct->rbb->iIw, rbCn, rbCnI );
1915
1916 ct->normal_mass = ct->rba->inv_mass + ct->rbb->inv_mass;
1917 ct->normal_mass += v3_dot( raCn, raCnI );
1918 ct->normal_mass += v3_dot( rbCn, rbCnI );
1919 ct->normal_mass = 1.0f/ct->normal_mass;
1920
1921 for( int j=0; j<2; j++ )
1922 {
1923 v3f raCtI, rbCtI;
1924 v3_cross( ct->t[j], ra, raCt );
1925 v3_cross( ct->t[j], rb, rbCt );
1926 m3x3_mulv( ct->rba->iIw, raCt, raCtI );
1927 m3x3_mulv( ct->rbb->iIw, rbCt, rbCtI );
1928
1929 ct->tangent_mass[j] = ct->rba->inv_mass + ct->rbb->inv_mass;
1930 ct->tangent_mass[j] += v3_dot( raCt, raCtI );
1931 ct->tangent_mass[j] += v3_dot( rbCt, rbCtI );
1932 ct->tangent_mass[j] = 1.0f/ct->tangent_mass[j];
1933 }
1934
1935 rb_debug_contact( ct );
1936 }
1937 }
1938
1939 /*
1940 * Creates relative contact velocity vector
1941 */
1942 VG_STATIC void rb_rcv( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb, v3f rv )
1943 {
1944 v3f rva, rvb;
1945 v3_cross( rba->w, ra, rva );
1946 v3_add( rba->v, rva, rva );
1947 v3_cross( rbb->w, rb, rvb );
1948 v3_add( rbb->v, rvb, rvb );
1949
1950 v3_sub( rva, rvb, rv );
1951 }
1952
1953 /*
1954 * Apply impulse to object
1955 */
1956 VG_STATIC void rb_linear_impulse( rigidbody *rb, v3f delta, v3f impulse )
1957 {
1958 /* linear */
1959 v3_muladds( rb->v, impulse, rb->inv_mass, rb->v );
1960
1961 /* Angular velocity */
1962 v3f wa;
1963 v3_cross( delta, impulse, wa );
1964
1965 m3x3_mulv( rb->iIw, wa, wa );
1966 v3_add( rb->w, wa, rb->w );
1967 }
1968
1969 /*
1970 * One iteration to solve the contact constraint
1971 */
1972 VG_STATIC void rb_solve_contacts( rb_ct *buf, int len )
1973 {
1974 for( int i=0; i<len; i++ )
1975 {
1976 struct contact *ct = &buf[i];
1977
1978 v3f rv, ra, rb;
1979 v3_sub( ct->co, ct->rba->co, ra );
1980 v3_sub( ct->co, ct->rbb->co, rb );
1981 rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
1982
1983 /* Friction */
1984 for( int j=0; j<2; j++ )
1985 {
1986 float f = k_friction * ct->norm_impulse,
1987 vt = v3_dot( rv, ct->t[j] ),
1988 lambda = ct->tangent_mass[j] * -vt;
1989
1990 float temp = ct->tangent_impulse[j];
1991 ct->tangent_impulse[j] = vg_clampf( temp + lambda, -f, f );
1992 lambda = ct->tangent_impulse[j] - temp;
1993
1994 v3f impulse;
1995 v3_muls( ct->t[j], lambda, impulse );
1996 rb_linear_impulse( ct->rba, ra, impulse );
1997
1998 v3_muls( ct->t[j], -lambda, impulse );
1999 rb_linear_impulse( ct->rbb, rb, impulse );
2000 }
2001
2002 /* Normal */
2003 rb_rcv( ct->rba, ct->rbb, ra, rb, rv );
2004 float vn = v3_dot( rv, ct->n ),
2005 lambda = ct->normal_mass * (-vn + ct->bias);
2006
2007 float temp = ct->norm_impulse;
2008 ct->norm_impulse = vg_maxf( temp + lambda, 0.0f );
2009 lambda = ct->norm_impulse - temp;
2010
2011 v3f impulse;
2012 v3_muls( ct->n, lambda, impulse );
2013 rb_linear_impulse( ct->rba, ra, impulse );
2014
2015 v3_muls( ct->n, -lambda, impulse );
2016 rb_linear_impulse( ct->rbb, rb, impulse );
2017 }
2018 }
2019
2020 /*
2021 * -----------------------------------------------------------------------------
2022 * Constraints
2023 * -----------------------------------------------------------------------------
2024 */
2025
2026 VG_STATIC void rb_debug_position_constraints( rb_constr_pos *buffer, int len )
2027 {
2028 for( int i=0; i<len; i++ )
2029 {
2030 rb_constr_pos *constr = &buffer[i];
2031 rigidbody *rba = constr->rba, *rbb = constr->rbb;
2032
2033 v3f wca, wcb;
2034 m3x3_mulv( rba->to_world, constr->lca, wca );
2035 m3x3_mulv( rbb->to_world, constr->lcb, wcb );
2036
2037 v3f p0, p1;
2038 v3_add( wca, rba->co, p0 );
2039 v3_add( wcb, rbb->co, p1 );
2040 vg_line_pt3( p0, 0.0025f, 0xff000000 );
2041 vg_line_pt3( p1, 0.0025f, 0xffffffff );
2042 vg_line2( p0, p1, 0xff000000, 0xffffffff );
2043 }
2044 }
2045
2046 VG_STATIC void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf,
2047 int len )
2048 {
2049 float size = 0.12f;
2050
2051 for( int i=0; i<len; i++ )
2052 {
2053 rb_constr_swingtwist *st = &buf[ i ];
2054
2055 v3f vx, vy, va, vxb, axis, center;
2056
2057 m3x3_mulv( st->rba->to_world, st->conevx, vx );
2058 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
2059 m3x3_mulv( st->rba->to_world, st->conevy, vy );
2060 m3x3_mulv( st->rbb->to_world, st->coneva, va );
2061 m4x3_mulv( st->rba->to_world, st->view_offset, center );
2062 v3_cross( vy, vx, axis );
2063
2064 /* Constraint violated ? */
2065 float fx = v3_dot( vx, va ), /* projection world */
2066 fy = v3_dot( vy, va ),
2067 fn = v3_dot( va, axis ),
2068
2069 rx = st->conevx[3], /* elipse radii */
2070 ry = st->conevy[3],
2071
2072 lx = fx/rx, /* projection local (fn==lz) */
2073 ly = fy/ry;
2074
2075 st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f);
2076 if( st->tangent_violation )
2077 {
2078 /* Calculate a good position and the axis to solve on */
2079 v2f closest, tangent,
2080 p = { fx/fabsf(fn), fy/fabsf(fn) };
2081
2082 closest_point_elipse( p, (v2f){rx,ry}, closest );
2083 tangent[0] = -closest[1] / (ry*ry);
2084 tangent[1] = closest[0] / (rx*rx);
2085 v2_normalize( tangent );
2086
2087 v3f v0, v1;
2088 v3_muladds( axis, vx, closest[0], v0 );
2089 v3_muladds( v0, vy, closest[1], v0 );
2090 v3_normalize( v0 );
2091
2092 v3_muls( vx, tangent[0], v1 );
2093 v3_muladds( v1, vy, tangent[1], v1 );
2094
2095 v3_copy( v0, st->tangent_target );
2096 v3_copy( v1, st->tangent_axis );
2097
2098 /* calculate mass */
2099 v3f aIw, bIw;
2100 m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw );
2101 m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw );
2102 st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) +
2103 v3_dot( st->tangent_axis, bIw ));
2104
2105 float angle = v3_dot( va, st->tangent_target );
2106 }
2107
2108 v3f refaxis;
2109 v3_cross( vy, va, refaxis ); /* our default rotation */
2110 v3_normalize( refaxis );
2111
2112 float angle = v3_dot( refaxis, vxb );
2113 st->axis_violation = fabsf(angle) < st->conet;
2114
2115 if( st->axis_violation )
2116 {
2117 v3f dir_test;
2118 v3_cross( refaxis, vxb, dir_test );
2119
2120 if( v3_dot(dir_test, va) < 0.0f )
2121 st->axis_violation = -st->axis_violation;
2122
2123 float newang = (float)st->axis_violation * acosf(st->conet-0.0001f);
2124
2125 v3f refaxis_up;
2126 v3_cross( va, refaxis, refaxis_up );
2127 v3_muls( refaxis_up, sinf(newang), st->axis_target );
2128 v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target );
2129
2130 /* calculate mass */
2131 v3_copy( va, st->axis );
2132 v3f aIw, bIw;
2133 m3x3_mulv( st->rba->iIw, st->axis, aIw );
2134 m3x3_mulv( st->rbb->iIw, st->axis, bIw );
2135 st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) +
2136 v3_dot( st->axis, bIw ));
2137 }
2138 }
2139 }
2140
2141 VG_STATIC void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf,
2142 int len )
2143 {
2144 float size = 0.12f;
2145
2146 for( int i=0; i<len; i++ )
2147 {
2148 rb_constr_swingtwist *st = &buf[ i ];
2149
2150 v3f vx, vxb, vy, va, axis, center;
2151
2152 m3x3_mulv( st->rba->to_world, st->conevx, vx );
2153 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
2154 m3x3_mulv( st->rba->to_world, st->conevy, vy );
2155 m3x3_mulv( st->rbb->to_world, st->coneva, va );
2156 m4x3_mulv( st->rba->to_world, st->view_offset, center );
2157 v3_cross( vy, vx, axis );
2158
2159 float rx = st->conevx[3], /* elipse radii */
2160 ry = st->conevy[3];
2161
2162 v3f p0, p1;
2163 v3_muladds( center, va, size, p1 );
2164 vg_line( center, p1, 0xffffffff );
2165 vg_line_pt3( p1, 0.00025f, 0xffffffff );
2166
2167 if( st->tangent_violation )
2168 {
2169 v3_muladds( center, st->tangent_target, size, p0 );
2170
2171 vg_line( center, p0, 0xff00ff00 );
2172 vg_line_pt3( p0, 0.00025f, 0xff00ff00 );
2173 vg_line( p1, p0, 0xff000000 );
2174 }
2175
2176 for( int x=0; x<32; x++ )
2177 {
2178 float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf,
2179 t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf,
2180 c0 = cosf( t0 ),
2181 s0 = sinf( t0 ),
2182 c1 = cosf( t1 ),
2183 s1 = sinf( t1 );
2184
2185 v3f v0, v1;
2186 v3_muladds( axis, vx, c0*rx, v0 );
2187 v3_muladds( v0, vy, s0*ry, v0 );
2188 v3_muladds( axis, vx, c1*rx, v1 );
2189 v3_muladds( v1, vy, s1*ry, v1 );
2190
2191 v3_normalize( v0 );
2192 v3_normalize( v1 );
2193
2194 v3_muladds( center, v0, size, p0 );
2195 v3_muladds( center, v1, size, p1 );
2196
2197 u32 col0r = fabsf(c0) * 255.0f,
2198 col0g = fabsf(s0) * 255.0f,
2199 col1r = fabsf(c1) * 255.0f,
2200 col1g = fabsf(s1) * 255.0f,
2201 col = st->tangent_violation? 0xff0000ff: 0xff000000,
2202 col0 = col | (col0r<<16) | (col0g << 8),
2203 col1 = col | (col1r<<16) | (col1g << 8);
2204
2205 vg_line2( center, p0, VG__NONE, col0 );
2206 vg_line2( p0, p1, col0, col1 );
2207 }
2208
2209 /* Draw twist */
2210 v3_muladds( center, va, size, p0 );
2211 v3_muladds( p0, vxb, size, p1 );
2212
2213 vg_line( p0, p1, 0xff0000ff );
2214
2215 if( st->axis_violation )
2216 {
2217 v3_muladds( p0, st->axis_target, size*1.25f, p1 );
2218 vg_line( p0, p1, 0xffffff00 );
2219 vg_line_pt3( p1, 0.0025f, 0xffffff80 );
2220 }
2221
2222 v3f refaxis;
2223 v3_cross( vy, va, refaxis ); /* our default rotation */
2224 v3_normalize( refaxis );
2225 v3f refaxis_up;
2226 v3_cross( va, refaxis, refaxis_up );
2227 float newang = acosf(st->conet-0.0001f);
2228
2229 v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 );
2230 v3_muladds( p1, refaxis, -cosf(newang)*size, p1 );
2231 vg_line( p0, p1, 0xff000000 );
2232
2233 v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 );
2234 v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 );
2235 vg_line( p0, p1, 0xff404040 );
2236 }
2237 }
2238
2239 /*
2240 * Solve a list of positional constraints
2241 */
2242 VG_STATIC void rb_solve_position_constraints( rb_constr_pos *buf, int len )
2243 {
2244 for( int i=0; i<len; i++ )
2245 {
2246 rb_constr_pos *constr = &buf[i];
2247 rigidbody *rba = constr->rba, *rbb = constr->rbb;
2248
2249 v3f wa, wb;
2250 m3x3_mulv( rba->to_world, constr->lca, wa );
2251 m3x3_mulv( rbb->to_world, constr->lcb, wb );
2252
2253 m3x3f ssra, ssrat, ssrb, ssrbt;
2254
2255 m3x3_skew_symetric( ssrat, wa );
2256 m3x3_skew_symetric( ssrbt, wb );
2257 m3x3_transpose( ssrat, ssra );
2258 m3x3_transpose( ssrbt, ssrb );
2259
2260 v3f b, b_wa, b_wb, b_a, b_b;
2261 m3x3_mulv( ssra, rba->w, b_wa );
2262 m3x3_mulv( ssrb, rbb->w, b_wb );
2263 v3_add( rba->v, b_wa, b );
2264 v3_sub( b, rbb->v, b );
2265 v3_sub( b, b_wb, b );
2266 v3_muls( b, -1.0f, b );
2267
2268 m3x3f invMa, invMb;
2269 m3x3_diagonal( invMa, rba->inv_mass );
2270 m3x3_diagonal( invMb, rbb->inv_mass );
2271
2272 m3x3f ia, ib;
2273 m3x3_mul( ssra, rba->iIw, ia );
2274 m3x3_mul( ia, ssrat, ia );
2275 m3x3_mul( ssrb, rbb->iIw, ib );
2276 m3x3_mul( ib, ssrbt, ib );
2277
2278 m3x3f cma, cmb;
2279 m3x3_add( invMa, ia, cma );
2280 m3x3_add( invMb, ib, cmb );
2281
2282 m3x3f A;
2283 m3x3_add( cma, cmb, A );
2284
2285 /* Solve Ax = b ( A^-1*b = x ) */
2286 v3f impulse;
2287 m3x3f invA;
2288 m3x3_inv( A, invA );
2289 m3x3_mulv( invA, b, impulse );
2290
2291 v3f delta_va, delta_wa, delta_vb, delta_wb;
2292 m3x3f iwa, iwb;
2293 m3x3_mul( rba->iIw, ssrat, iwa );
2294 m3x3_mul( rbb->iIw, ssrbt, iwb );
2295
2296 m3x3_mulv( invMa, impulse, delta_va );
2297 m3x3_mulv( invMb, impulse, delta_vb );
2298 m3x3_mulv( iwa, impulse, delta_wa );
2299 m3x3_mulv( iwb, impulse, delta_wb );
2300
2301 v3_add( rba->v, delta_va, rba->v );
2302 v3_add( rba->w, delta_wa, rba->w );
2303 v3_sub( rbb->v, delta_vb, rbb->v );
2304 v3_sub( rbb->w, delta_wb, rbb->w );
2305 }
2306 }
2307
2308 VG_STATIC void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf,
2309 int len )
2310 {
2311 float size = 0.12f;
2312
2313 for( int i=0; i<len; i++ )
2314 {
2315 rb_constr_swingtwist *st = &buf[ i ];
2316
2317 if( !st->axis_violation )
2318 continue;
2319
2320 float rv = v3_dot( st->axis, st->rbb->w ) -
2321 v3_dot( st->axis, st->rba->w );
2322
2323 if( rv * (float)st->axis_violation > 0.0f )
2324 continue;
2325
2326 v3f impulse, wa, wb;
2327 v3_muls( st->axis, rv*st->axis_mass, impulse );
2328 m3x3_mulv( st->rba->iIw, impulse, wa );
2329 v3_add( st->rba->w, wa, st->rba->w );
2330
2331 v3_muls( impulse, -1.0f, impulse );
2332 m3x3_mulv( st->rbb->iIw, impulse, wb );
2333 v3_add( st->rbb->w, wb, st->rbb->w );
2334
2335 float rv2 = v3_dot( st->axis, st->rbb->w ) -
2336 v3_dot( st->axis, st->rba->w );
2337 }
2338
2339 for( int i=0; i<len; i++ )
2340 {
2341 rb_constr_swingtwist *st = &buf[ i ];
2342
2343 if( !st->tangent_violation )
2344 continue;
2345
2346 float rv = v3_dot( st->tangent_axis, st->rbb->w ) -
2347 v3_dot( st->tangent_axis, st->rba->w );
2348
2349 if( rv > 0.0f )
2350 continue;
2351
2352 v3f impulse, wa, wb;
2353 v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse );
2354 m3x3_mulv( st->rba->iIw, impulse, wa );
2355 v3_add( st->rba->w, wa, st->rba->w );
2356
2357 v3_muls( impulse, -1.0f, impulse );
2358 m3x3_mulv( st->rbb->iIw, impulse, wb );
2359 v3_add( st->rbb->w, wb, st->rbb->w );
2360
2361 float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) -
2362 v3_dot( st->tangent_axis, st->rba->w );
2363 }
2364 }
2365
2366 VG_STATIC void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb,
2367 v3f ra, v3f rb )
2368 {
2369 m3x3f ssra, ssrb, ssrat, ssrbt;
2370 m3x3f cma, cmb;
2371
2372 m3x3_skew_symetric( ssrat, ra );
2373 m3x3_skew_symetric( ssrbt, rb );
2374 m3x3_transpose( ssrat, ssra );
2375 m3x3_transpose( ssrbt, ssrb );
2376
2377 m3x3_mul( ssra, rba->iIw, cma );
2378 m3x3_mul( cma, ssrat, cma );
2379 m3x3_mul( ssrb, rbb->iIw, cmb );
2380 m3x3_mul( cmb, ssrbt, cmb );
2381
2382 m3x3f A, invA;
2383 m3x3_add( cma, cmb, A );
2384 m3x3_inv( A, invA );
2385
2386 v3f b_wa, b_wb, b;
2387 m3x3_mulv( ssra, rba->w, b_wa );
2388 m3x3_mulv( ssrb, rbb->w, b_wb );
2389 v3_add( b_wa, b_wb, b );
2390 v3_negate( b, b );
2391
2392 v3f impulse;
2393 m3x3_mulv( invA, b, impulse );
2394
2395 v3f delta_wa, delta_wb;
2396 m3x3f iwa, iwb;
2397 m3x3_mul( rba->iIw, ssrat, iwa );
2398 m3x3_mul( rbb->iIw, ssrbt, iwb );
2399 m3x3_mulv( iwa, impulse, delta_wa );
2400 m3x3_mulv( iwb, impulse, delta_wb );
2401 v3_add( rba->w, delta_wa, rba->w );
2402 v3_sub( rbb->w, delta_wb, rbb->w );
2403 }
2404
2405 /*
2406 * Correct position constraint drift errors
2407 * [ 0.0 <= amt <= 1.0 ]: the correction amount
2408 */
2409 VG_STATIC void rb_correct_position_constraints( rb_constr_pos *buf, int len,
2410 float amt )
2411 {
2412 for( int i=0; i<len; i++ )
2413 {
2414 rb_constr_pos *constr = &buf[i];
2415 rigidbody *rba = constr->rba, *rbb = constr->rbb;
2416
2417 v3f p0, p1, d;
2418 m3x3_mulv( rba->to_world, constr->lca, p0 );
2419 m3x3_mulv( rbb->to_world, constr->lcb, p1 );
2420 v3_add( rba->co, p0, p0 );
2421 v3_add( rbb->co, p1, p1 );
2422 v3_sub( p1, p0, d );
2423
2424 v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
2425 rb_update_transform( rbb );
2426 }
2427 }
2428
2429 VG_STATIC void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
2430 int len, float amt )
2431 {
2432 for( int i=0; i<len; i++ )
2433 {
2434 rb_constr_swingtwist *st = &buf[i];
2435
2436 if( !st->tangent_violation )
2437 continue;
2438
2439 v3f va;
2440 m3x3_mulv( st->rbb->to_world, st->coneva, va );
2441
2442 float angle = v3_dot( va, st->tangent_target );
2443
2444 if( fabsf(angle) < 0.9999f )
2445 {
2446 v3f axis;
2447 v3_cross( va, st->tangent_target, axis );
2448
2449 v4f correction;
2450 q_axis_angle( correction, axis, acosf(angle) * amt );
2451 q_mul( correction, st->rbb->q, st->rbb->q );
2452 rb_update_transform( st->rbb );
2453 }
2454 }
2455
2456 for( int i=0; i<len; i++ )
2457 {
2458 rb_constr_swingtwist *st = &buf[i];
2459
2460 if( !st->axis_violation )
2461 continue;
2462
2463 v3f vxb;
2464 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
2465
2466 float angle = v3_dot( vxb, st->axis_target );
2467
2468 if( fabsf(angle) < 0.9999f )
2469 {
2470 v3f axis;
2471 v3_cross( vxb, st->axis_target, axis );
2472
2473 v4f correction;
2474 q_axis_angle( correction, axis, acosf(angle) * amt );
2475 q_mul( correction, st->rbb->q, st->rbb->q );
2476 rb_update_transform( st->rbb );
2477 }
2478 }
2479 }
2480
2481 VG_STATIC void rb_correct_contact_constraints( rb_ct *buf, int len, float amt )
2482 {
2483 for( int i=0; i<len; i++ )
2484 {
2485 rb_ct *ct = &buf[i];
2486 rigidbody *rba = ct->rba,
2487 *rbb = ct->rbb;
2488
2489 float mass_total = 1.0f / (rba->inv_mass + rbb->inv_mass);
2490
2491 v3_muladds( rba->co, ct->n, -mass_total * rba->inv_mass, rba->co );
2492 v3_muladds( rbb->co, ct->n, mass_total * rbb->inv_mass, rbb->co );
2493 }
2494 }
2495
2496
2497 /*
2498 * Effectors
2499 */
2500
2501 VG_STATIC void rb_effect_simple_bouyency( rigidbody *ra, v4f plane,
2502 float amt, float drag )
2503 {
2504 /* float */
2505 float depth = v3_dot( plane, ra->co ) - plane[3],
2506 lambda = vg_clampf( -depth, 0.0f, 1.0f ) * amt;
2507
2508 v3_muladds( ra->v, plane, lambda * k_rb_delta, ra->v );
2509
2510 if( depth < 0.0f )
2511 v3_muls( ra->v, 1.0f-(drag*k_rb_delta), ra->v );
2512 }
2513
2514 /*
2515 * -----------------------------------------------------------------------------
2516 * BVH implementation, this is ONLY for VG_STATIC rigidbodies, its to slow for
2517 * realtime use.
2518 * -----------------------------------------------------------------------------
2519 */
2520
2521 VG_STATIC void rb_bh_expand_bound( void *user, boxf bound, u32 item_index )
2522 {
2523 rigidbody *rb = &((rigidbody *)user)[ item_index ];
2524 box_concat( bound, rb->bbx_world );
2525 }
2526
2527 VG_STATIC float rb_bh_centroid( void *user, u32 item_index, int axis )
2528 {
2529 rigidbody *rb = &((rigidbody *)user)[ item_index ];
2530 return (rb->bbx_world[axis][0] + rb->bbx_world[1][axis]) * 0.5f;
2531 }
2532
2533 VG_STATIC void rb_bh_swap( void *user, u32 ia, u32 ib )
2534 {
2535 rigidbody temp, *rba, *rbb;
2536 rba = &((rigidbody *)user)[ ia ];
2537 rbb = &((rigidbody *)user)[ ib ];
2538
2539 temp = *rba;
2540 *rba = *rbb;
2541 *rbb = temp;
2542 }
2543
2544 VG_STATIC void rb_bh_debug( void *user, u32 item_index )
2545 {
2546 rigidbody *rb = &((rigidbody *)user)[ item_index ];
2547 rb_debug( rb, 0xff00ffff );
2548 }
2549
2550 VG_STATIC bh_system bh_system_rigidbodies =
2551 {
2552 .expand_bound = rb_bh_expand_bound,
2553 .item_centroid = rb_bh_centroid,
2554 .item_swap = rb_bh_swap,
2555 .item_debug = rb_bh_debug,
2556 .cast_ray = NULL
2557 };
2558
2559 #endif /* RIGIDBODY_H */
Skaterift source tree