build system revision
[vg.git] / vg_rigidbody_constraints.c
1 #pragma once
2 #include "vg_rigidbody.h"
3 #include "vg_rigidbody_constraints.h"
4 #include "vg_m.h"
5 #include "vg_lines.h"
6
7 /*
8 * -----------------------------------------------------------------------------
9 * Constraints
10 * -----------------------------------------------------------------------------
11 */
12
13 void rb_debug_position_constraints( rb_constr_pos *buffer, int len )
14 {
15 for( int i=0; i<len; i++ ){
16 rb_constr_pos *constr = &buffer[i];
17 rigidbody *rba = constr->rba, *rbb = constr->rbb;
18
19 v3f wca, wcb;
20 m3x3_mulv( rba->to_world, constr->lca, wca );
21 m3x3_mulv( rbb->to_world, constr->lcb, wcb );
22
23 v3f p0, p1;
24 v3_add( wca, rba->co, p0 );
25 v3_add( wcb, rbb->co, p1 );
26 vg_line_point( p0, 0.0025f, 0xff000000 );
27 vg_line_point( p1, 0.0025f, 0xffffffff );
28 vg_line2( p0, p1, 0xff000000, 0xffffffff );
29 }
30 }
31
32 void rb_presolve_swingtwist_constraints( rb_constr_swingtwist *buf, int len )
33 {
34 for( int i=0; i<len; i++ ){
35 rb_constr_swingtwist *st = &buf[ i ];
36
37 v3f vx, vy, va, vxb, axis, center;
38
39 m3x3_mulv( st->rba->to_world, st->conevx, vx );
40 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
41 m3x3_mulv( st->rba->to_world, st->conevy, vy );
42 m3x3_mulv( st->rbb->to_world, st->coneva, va );
43 m4x3_mulv( st->rba->to_world, st->view_offset, center );
44 v3_cross( vy, vx, axis );
45
46 /* Constraint violated ? */
47 float fx = v3_dot( vx, va ), /* projection world */
48 fy = v3_dot( vy, va ),
49 fn = v3_dot( va, axis ),
50
51 rx = st->conevx[3], /* elipse radii */
52 ry = st->conevy[3],
53
54 lx = fx/rx, /* projection local (fn==lz) */
55 ly = fy/ry;
56
57 st->tangent_violation = ((lx*lx + ly*ly) > fn*fn) || (fn <= 0.0f);
58 if( st->tangent_violation ){
59 /* Calculate a good position and the axis to solve on */
60 v2f closest, tangent,
61 p = { fx/fabsf(fn), fy/fabsf(fn) };
62
63 closest_point_elipse( p, (v2f){rx,ry}, closest );
64 tangent[0] = -closest[1] / (ry*ry);
65 tangent[1] = closest[0] / (rx*rx);
66 v2_normalize( tangent );
67
68 v3f v0, v1;
69 v3_muladds( axis, vx, closest[0], v0 );
70 v3_muladds( v0, vy, closest[1], v0 );
71 v3_normalize( v0 );
72
73 v3_muls( vx, tangent[0], v1 );
74 v3_muladds( v1, vy, tangent[1], v1 );
75
76 v3_copy( v0, st->tangent_target );
77 v3_copy( v1, st->tangent_axis );
78
79 /* calculate mass */
80 v3f aIw, bIw;
81 m3x3_mulv( st->rba->iIw, st->tangent_axis, aIw );
82 m3x3_mulv( st->rbb->iIw, st->tangent_axis, bIw );
83 st->tangent_mass = 1.0f / (v3_dot( st->tangent_axis, aIw ) +
84 v3_dot( st->tangent_axis, bIw ));
85
86 float angle = v3_dot( va, st->tangent_target );
87 }
88
89 v3f refaxis;
90 v3_cross( vy, va, refaxis ); /* our default rotation */
91 v3_normalize( refaxis );
92
93 float angle = v3_dot( refaxis, vxb );
94 st->axis_violation = fabsf(angle) < st->conet;
95
96 if( st->axis_violation ){
97 v3f dir_test;
98 v3_cross( refaxis, vxb, dir_test );
99
100 if( v3_dot(dir_test, va) < 0.0f )
101 st->axis_violation = -st->axis_violation;
102
103 float newang = (float)st->axis_violation * acosf(st->conet-0.0001f);
104
105 v3f refaxis_up;
106 v3_cross( va, refaxis, refaxis_up );
107 v3_muls( refaxis_up, sinf(newang), st->axis_target );
108 v3_muladds( st->axis_target, refaxis, -cosf(newang), st->axis_target );
109
110 /* calculate mass */
111 v3_copy( va, st->axis );
112 v3f aIw, bIw;
113 m3x3_mulv( st->rba->iIw, st->axis, aIw );
114 m3x3_mulv( st->rbb->iIw, st->axis, bIw );
115 st->axis_mass = 1.0f / (v3_dot( st->axis, aIw ) +
116 v3_dot( st->axis, bIw ));
117 }
118 }
119 }
120
121 void rb_debug_swingtwist_constraints( rb_constr_swingtwist *buf, int len )
122 {
123 float size = 0.12f;
124
125 for( int i=0; i<len; i++ ){
126 rb_constr_swingtwist *st = &buf[ i ];
127
128 v3f vx, vxb, vy, va, axis, center;
129
130 m3x3_mulv( st->rba->to_world, st->conevx, vx );
131 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
132 m3x3_mulv( st->rba->to_world, st->conevy, vy );
133 m3x3_mulv( st->rbb->to_world, st->coneva, va );
134 m4x3_mulv( st->rba->to_world, st->view_offset, center );
135 v3_cross( vy, vx, axis );
136
137 float rx = st->conevx[3], /* elipse radii */
138 ry = st->conevy[3];
139
140 v3f p0, p1;
141 v3_muladds( center, va, size, p1 );
142 vg_line( center, p1, 0xffffffff );
143 vg_line_point( p1, 0.00025f, 0xffffffff );
144
145 if( st->tangent_violation ){
146 v3_muladds( center, st->tangent_target, size, p0 );
147
148 vg_line( center, p0, 0xff00ff00 );
149 vg_line_point( p0, 0.00025f, 0xff00ff00 );
150 vg_line( p1, p0, 0xff000000 );
151 }
152
153 for( int x=0; x<32; x++ ){
154 float t0 = ((float)x * (1.0f/32.0f)) * VG_TAUf,
155 t1 = (((float)x+1.0f) * (1.0f/32.0f)) * VG_TAUf,
156 c0 = cosf( t0 ),
157 s0 = sinf( t0 ),
158 c1 = cosf( t1 ),
159 s1 = sinf( t1 );
160
161 v3f v0, v1;
162 v3_muladds( axis, vx, c0*rx, v0 );
163 v3_muladds( v0, vy, s0*ry, v0 );
164 v3_muladds( axis, vx, c1*rx, v1 );
165 v3_muladds( v1, vy, s1*ry, v1 );
166
167 v3_normalize( v0 );
168 v3_normalize( v1 );
169
170 v3_muladds( center, v0, size, p0 );
171 v3_muladds( center, v1, size, p1 );
172
173 u32 col0r = fabsf(c0) * 255.0f,
174 col0g = fabsf(s0) * 255.0f,
175 col1r = fabsf(c1) * 255.0f,
176 col1g = fabsf(s1) * 255.0f,
177 col = st->tangent_violation? 0xff0000ff: 0xff000000,
178 col0 = col | (col0r<<16) | (col0g << 8),
179 col1 = col | (col1r<<16) | (col1g << 8);
180
181 vg_line2( center, p0, VG__NONE, col0 );
182 vg_line2( p0, p1, col0, col1 );
183 }
184
185 /* Draw twist */
186 v3_muladds( center, va, size, p0 );
187 v3_muladds( p0, vxb, size, p1 );
188
189 vg_line( p0, p1, 0xff0000ff );
190
191 if( st->axis_violation ){
192 v3_muladds( p0, st->axis_target, size*1.25f, p1 );
193 vg_line( p0, p1, 0xffffff00 );
194 vg_line_point( p1, 0.0025f, 0xffffff80 );
195 }
196
197 v3f refaxis;
198 v3_cross( vy, va, refaxis ); /* our default rotation */
199 v3_normalize( refaxis );
200 v3f refaxis_up;
201 v3_cross( va, refaxis, refaxis_up );
202 float newang = acosf(st->conet-0.0001f);
203
204 v3_muladds( p0, refaxis_up, sinf(newang)*size, p1 );
205 v3_muladds( p1, refaxis, -cosf(newang)*size, p1 );
206 vg_line( p0, p1, 0xff000000 );
207
208 v3_muladds( p0, refaxis_up, sinf(-newang)*size, p1 );
209 v3_muladds( p1, refaxis, -cosf(-newang)*size, p1 );
210 vg_line( p0, p1, 0xff404040 );
211 }
212 }
213
214 void rb_solve_position_constraints( rb_constr_pos *buf, int len )
215 {
216 for( int i=0; i<len; i++ ){
217 rb_constr_pos *constr = &buf[i];
218 rigidbody *rba = constr->rba, *rbb = constr->rbb;
219
220 v3f wa, wb;
221 m3x3_mulv( rba->to_world, constr->lca, wa );
222 m3x3_mulv( rbb->to_world, constr->lcb, wb );
223
224 m3x3f ssra, ssrat, ssrb, ssrbt;
225
226 m3x3_skew_symetric( ssrat, wa );
227 m3x3_skew_symetric( ssrbt, wb );
228 m3x3_transpose( ssrat, ssra );
229 m3x3_transpose( ssrbt, ssrb );
230
231 v3f b, b_wa, b_wb, b_a, b_b;
232 m3x3_mulv( ssra, rba->w, b_wa );
233 m3x3_mulv( ssrb, rbb->w, b_wb );
234 v3_add( rba->v, b_wa, b );
235 v3_sub( b, rbb->v, b );
236 v3_sub( b, b_wb, b );
237 v3_muls( b, -1.0f, b );
238
239 m3x3f invMa, invMb;
240 m3x3_diagonal( invMa, rba->inv_mass );
241 m3x3_diagonal( invMb, rbb->inv_mass );
242
243 m3x3f ia, ib;
244 m3x3_mul( ssra, rba->iIw, ia );
245 m3x3_mul( ia, ssrat, ia );
246 m3x3_mul( ssrb, rbb->iIw, ib );
247 m3x3_mul( ib, ssrbt, ib );
248
249 m3x3f cma, cmb;
250 m3x3_add( invMa, ia, cma );
251 m3x3_add( invMb, ib, cmb );
252
253 m3x3f A;
254 m3x3_add( cma, cmb, A );
255
256 /* Solve Ax = b ( A^-1*b = x ) */
257 v3f impulse;
258 m3x3f invA;
259 m3x3_inv( A, invA );
260 m3x3_mulv( invA, b, impulse );
261
262 v3f delta_va, delta_wa, delta_vb, delta_wb;
263 m3x3f iwa, iwb;
264 m3x3_mul( rba->iIw, ssrat, iwa );
265 m3x3_mul( rbb->iIw, ssrbt, iwb );
266
267 m3x3_mulv( invMa, impulse, delta_va );
268 m3x3_mulv( invMb, impulse, delta_vb );
269 m3x3_mulv( iwa, impulse, delta_wa );
270 m3x3_mulv( iwb, impulse, delta_wb );
271
272 v3_add( rba->v, delta_va, rba->v );
273 v3_add( rba->w, delta_wa, rba->w );
274 v3_sub( rbb->v, delta_vb, rbb->v );
275 v3_sub( rbb->w, delta_wb, rbb->w );
276 }
277 }
278
279 void rb_solve_swingtwist_constraints( rb_constr_swingtwist *buf, int len )
280 {
281 for( int i=0; i<len; i++ ){
282 rb_constr_swingtwist *st = &buf[ i ];
283
284 if( !st->axis_violation )
285 continue;
286
287 float rv = v3_dot( st->axis, st->rbb->w ) -
288 v3_dot( st->axis, st->rba->w );
289
290 if( rv * (float)st->axis_violation > 0.0f )
291 continue;
292
293 v3f impulse, wa, wb;
294 v3_muls( st->axis, rv*st->axis_mass, impulse );
295 m3x3_mulv( st->rba->iIw, impulse, wa );
296 v3_add( st->rba->w, wa, st->rba->w );
297
298 v3_muls( impulse, -1.0f, impulse );
299 m3x3_mulv( st->rbb->iIw, impulse, wb );
300 v3_add( st->rbb->w, wb, st->rbb->w );
301
302 float rv2 = v3_dot( st->axis, st->rbb->w ) -
303 v3_dot( st->axis, st->rba->w );
304 }
305
306 for( int i=0; i<len; i++ ){
307 rb_constr_swingtwist *st = &buf[ i ];
308
309 if( !st->tangent_violation )
310 continue;
311
312 float rv = v3_dot( st->tangent_axis, st->rbb->w ) -
313 v3_dot( st->tangent_axis, st->rba->w );
314
315 if( rv > 0.0f )
316 continue;
317
318 v3f impulse, wa, wb;
319 v3_muls( st->tangent_axis, rv*st->tangent_mass, impulse );
320 m3x3_mulv( st->rba->iIw, impulse, wa );
321 v3_add( st->rba->w, wa, st->rba->w );
322
323 v3_muls( impulse, -1.0f, impulse );
324 m3x3_mulv( st->rbb->iIw, impulse, wb );
325 v3_add( st->rbb->w, wb, st->rbb->w );
326
327 float rv2 = v3_dot( st->tangent_axis, st->rbb->w ) -
328 v3_dot( st->tangent_axis, st->rba->w );
329 }
330 }
331
332 /* debugging */
333 void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist *buf, u32 len )
334 {
335 for( int i=0; i<len; i++ ){
336 rb_constr_swingtwist *st = &buf[ i ];
337
338 if( !st->axis_violation ){
339 st->conv_axis = 0.0f;
340 continue;
341 }
342
343 f32 rv = v3_dot( st->axis, st->rbb->w ) -
344 v3_dot( st->axis, st->rba->w );
345
346 if( rv * (f32)st->axis_violation > 0.0f )
347 st->conv_axis = 0.0f;
348 else
349 st->conv_axis = rv;
350 }
351
352 for( int i=0; i<len; i++ ){
353 rb_constr_swingtwist *st = &buf[ i ];
354
355 if( !st->tangent_violation ){
356 st->conv_tangent = 0.0f;
357 continue;
358 }
359
360 f32 rv = v3_dot( st->tangent_axis, st->rbb->w ) -
361 v3_dot( st->tangent_axis, st->rba->w );
362
363 if( rv > 0.0f )
364 st->conv_tangent = 0.0f;
365 else
366 st->conv_tangent = rv;
367 }
368 }
369
370 void rb_solve_constr_angle( rigidbody *rba, rigidbody *rbb, v3f ra, v3f rb )
371 {
372 m3x3f ssra, ssrb, ssrat, ssrbt;
373 m3x3f cma, cmb;
374
375 m3x3_skew_symetric( ssrat, ra );
376 m3x3_skew_symetric( ssrbt, rb );
377 m3x3_transpose( ssrat, ssra );
378 m3x3_transpose( ssrbt, ssrb );
379
380 m3x3_mul( ssra, rba->iIw, cma );
381 m3x3_mul( cma, ssrat, cma );
382 m3x3_mul( ssrb, rbb->iIw, cmb );
383 m3x3_mul( cmb, ssrbt, cmb );
384
385 m3x3f A, invA;
386 m3x3_add( cma, cmb, A );
387 m3x3_inv( A, invA );
388
389 v3f b_wa, b_wb, b;
390 m3x3_mulv( ssra, rba->w, b_wa );
391 m3x3_mulv( ssrb, rbb->w, b_wb );
392 v3_add( b_wa, b_wb, b );
393 v3_negate( b, b );
394
395 v3f impulse;
396 m3x3_mulv( invA, b, impulse );
397
398 v3f delta_wa, delta_wb;
399 m3x3f iwa, iwb;
400 m3x3_mul( rba->iIw, ssrat, iwa );
401 m3x3_mul( rbb->iIw, ssrbt, iwb );
402 m3x3_mulv( iwa, impulse, delta_wa );
403 m3x3_mulv( iwb, impulse, delta_wb );
404 v3_add( rba->w, delta_wa, rba->w );
405 v3_sub( rbb->w, delta_wb, rbb->w );
406 }
407
408 void rb_correct_position_constraints( rb_constr_pos *buf, int len, f32 amt )
409 {
410 for( int i=0; i<len; i++ ){
411 rb_constr_pos *constr = &buf[i];
412 rigidbody *rba = constr->rba, *rbb = constr->rbb;
413
414 v3f p0, p1, d;
415 m3x3_mulv( rba->to_world, constr->lca, p0 );
416 m3x3_mulv( rbb->to_world, constr->lcb, p1 );
417 v3_add( rba->co, p0, p0 );
418 v3_add( rbb->co, p1, p1 );
419 v3_sub( p1, p0, d );
420
421 #if 1
422 v3_muladds( rbb->co, d, -1.0f * amt, rbb->co );
423 rb_update_matrices( rbb );
424 #else
425 f32 mt = 1.0f/(rba->inv_mass+rbb->inv_mass),
426 a = mt * (k_phys_baumgarte/k_rb_delta);
427
428 v3_muladds( rba->v, d, a* rba->inv_mass, rba->v );
429 v3_muladds( rbb->v, d, a*-rbb->inv_mass, rbb->v );
430 #endif
431 }
432 }
433
434 void rb_correct_swingtwist_constraints( rb_constr_swingtwist *buf,
435 int len, float amt )
436 {
437 for( int i=0; i<len; i++ ){
438 rb_constr_swingtwist *st = &buf[i];
439
440 if( !st->tangent_violation )
441 continue;
442
443 v3f va;
444 m3x3_mulv( st->rbb->to_world, st->coneva, va );
445
446 f32 angle = v3_dot( va, st->tangent_target );
447
448 if( fabsf(angle) < 0.9999f ){
449 v3f axis;
450 v3_cross( va, st->tangent_target, axis );
451 #if 1
452 angle = acosf(angle) * amt;
453 v4f correction;
454 q_axis_angle( correction, axis, angle );
455 q_mul( correction, st->rbb->q, st->rbb->q );
456 q_normalize( st->rbb->q );
457 rb_update_matrices( st->rbb );
458 #else
459 f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
460 wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
461 //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w );
462 v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
463 #endif
464 }
465 }
466
467 for( int i=0; i<len; i++ ){
468 rb_constr_swingtwist *st = &buf[i];
469
470 if( !st->axis_violation )
471 continue;
472
473 v3f vxb;
474 m3x3_mulv( st->rbb->to_world, st->conevxb, vxb );
475
476 f32 angle = v3_dot( vxb, st->axis_target );
477
478 if( fabsf(angle) < 0.9999f ){
479 v3f axis;
480 v3_cross( vxb, st->axis_target, axis );
481
482 #if 1
483 angle = acosf(angle) * amt;
484 v4f correction;
485 q_axis_angle( correction, axis, angle );
486 q_mul( correction, st->rbb->q, st->rbb->q );
487 q_normalize( st->rbb->q );
488 rb_update_matrices( st->rbb );
489 #else
490 f32 mt = 1.0f/(st->rba->inv_mass+st->rbb->inv_mass),
491 wa = mt * acosf(angle) * (k_phys_baumgarte/k_rb_delta);
492 //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w );
493 v3_muladds( st->rbb->w, axis, wa* st->rbb->inv_mass, st->rbb->w );
494 #endif
495 }
496 }
497 }