2 #include "vg_rigidbody.h"
4 typedef struct rb_constr_pos rb_constr_pos
;
5 typedef struct rb_constr_swingtwist rb_constr_swingtwist
;
12 struct rb_constr_swingtwist
{
15 v4f conevx
, conevy
; /* relative to rba */
16 v3f view_offset
, /* relative to rba */
17 coneva
, conevxb
;/* relative to rbb */
19 int tangent_violation
, axis_violation
;
20 v3f axis
, tangent_axis
, tangent_target
, axis_target
;
23 float tangent_mass
, axis_mass
;
25 f32 conv_tangent
, conv_axis
;
29 * -----------------------------------------------------------------------------
31 * -----------------------------------------------------------------------------
34 static void rb_debug_position_constraints( rb_constr_pos
*buffer
, int len
){
35 for( int i
=0; i
<len
; i
++ ){
36 rb_constr_pos
*constr
= &buffer
[i
];
37 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
40 m3x3_mulv( rba
->to_world
, constr
->lca
, wca
);
41 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wcb
);
44 v3_add( wca
, rba
->co
, p0
);
45 v3_add( wcb
, rbb
->co
, p1
);
46 vg_line_point( p0
, 0.0025f
, 0xff000000 );
47 vg_line_point( p1
, 0.0025f
, 0xffffffff );
48 vg_line2( p0
, p1
, 0xff000000, 0xffffffff );
52 static void rb_presolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
54 for( int i
=0; i
<len
; i
++ ){
55 rb_constr_swingtwist
*st
= &buf
[ i
];
57 v3f vx
, vy
, va
, vxb
, axis
, center
;
59 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
60 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
61 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
62 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
63 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
64 v3_cross( vy
, vx
, axis
);
66 /* Constraint violated ? */
67 float fx
= v3_dot( vx
, va
), /* projection world */
68 fy
= v3_dot( vy
, va
),
69 fn
= v3_dot( va
, axis
),
71 rx
= st
->conevx
[3], /* elipse radii */
74 lx
= fx
/rx
, /* projection local (fn==lz) */
77 st
->tangent_violation
= ((lx
*lx
+ ly
*ly
) > fn
*fn
) || (fn
<= 0.0f
);
78 if( st
->tangent_violation
){
79 /* Calculate a good position and the axis to solve on */
81 p
= { fx
/fabsf(fn
), fy
/fabsf(fn
) };
83 closest_point_elipse( p
, (v2f
){rx
,ry
}, closest
);
84 tangent
[0] = -closest
[1] / (ry
*ry
);
85 tangent
[1] = closest
[0] / (rx
*rx
);
86 v2_normalize( tangent
);
89 v3_muladds( axis
, vx
, closest
[0], v0
);
90 v3_muladds( v0
, vy
, closest
[1], v0
);
93 v3_muls( vx
, tangent
[0], v1
);
94 v3_muladds( v1
, vy
, tangent
[1], v1
);
96 v3_copy( v0
, st
->tangent_target
);
97 v3_copy( v1
, st
->tangent_axis
);
101 m3x3_mulv( st
->rba
->iIw
, st
->tangent_axis
, aIw
);
102 m3x3_mulv( st
->rbb
->iIw
, st
->tangent_axis
, bIw
);
103 st
->tangent_mass
= 1.0f
/ (v3_dot( st
->tangent_axis
, aIw
) +
104 v3_dot( st
->tangent_axis
, bIw
));
106 float angle
= v3_dot( va
, st
->tangent_target
);
110 v3_cross( vy
, va
, refaxis
); /* our default rotation */
111 v3_normalize( refaxis
);
113 float angle
= v3_dot( refaxis
, vxb
);
114 st
->axis_violation
= fabsf(angle
) < st
->conet
;
116 if( st
->axis_violation
){
118 v3_cross( refaxis
, vxb
, dir_test
);
120 if( v3_dot(dir_test
, va
) < 0.0f
)
121 st
->axis_violation
= -st
->axis_violation
;
123 float newang
= (float)st
->axis_violation
* acosf(st
->conet
-0.0001f
);
126 v3_cross( va
, refaxis
, refaxis_up
);
127 v3_muls( refaxis_up
, sinf(newang
), st
->axis_target
);
128 v3_muladds( st
->axis_target
, refaxis
, -cosf(newang
), st
->axis_target
);
131 v3_copy( va
, st
->axis
);
133 m3x3_mulv( st
->rba
->iIw
, st
->axis
, aIw
);
134 m3x3_mulv( st
->rbb
->iIw
, st
->axis
, bIw
);
135 st
->axis_mass
= 1.0f
/ (v3_dot( st
->axis
, aIw
) +
136 v3_dot( st
->axis
, bIw
));
141 static void rb_debug_swingtwist_constraints( rb_constr_swingtwist
*buf
,
145 for( int i
=0; i
<len
; i
++ ){
146 rb_constr_swingtwist
*st
= &buf
[ i
];
148 v3f vx
, vxb
, vy
, va
, axis
, center
;
150 m3x3_mulv( st
->rba
->to_world
, st
->conevx
, vx
);
151 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
152 m3x3_mulv( st
->rba
->to_world
, st
->conevy
, vy
);
153 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
154 m4x3_mulv( st
->rba
->to_world
, st
->view_offset
, center
);
155 v3_cross( vy
, vx
, axis
);
157 float rx
= st
->conevx
[3], /* elipse radii */
161 v3_muladds( center
, va
, size
, p1
);
162 vg_line( center
, p1
, 0xffffffff );
163 vg_line_point( p1
, 0.00025f
, 0xffffffff );
165 if( st
->tangent_violation
){
166 v3_muladds( center
, st
->tangent_target
, size
, p0
);
168 vg_line( center
, p0
, 0xff00ff00 );
169 vg_line_point( p0
, 0.00025f
, 0xff00ff00 );
170 vg_line( p1
, p0
, 0xff000000 );
173 for( int x
=0; x
<32; x
++ ){
174 float t0
= ((float)x
* (1.0f
/32.0f
)) * VG_TAUf
,
175 t1
= (((float)x
+1.0f
) * (1.0f
/32.0f
)) * VG_TAUf
,
182 v3_muladds( axis
, vx
, c0
*rx
, v0
);
183 v3_muladds( v0
, vy
, s0
*ry
, v0
);
184 v3_muladds( axis
, vx
, c1
*rx
, v1
);
185 v3_muladds( v1
, vy
, s1
*ry
, v1
);
190 v3_muladds( center
, v0
, size
, p0
);
191 v3_muladds( center
, v1
, size
, p1
);
193 u32 col0r
= fabsf(c0
) * 255.0f
,
194 col0g
= fabsf(s0
) * 255.0f
,
195 col1r
= fabsf(c1
) * 255.0f
,
196 col1g
= fabsf(s1
) * 255.0f
,
197 col
= st
->tangent_violation
? 0xff0000ff: 0xff000000,
198 col0
= col
| (col0r
<<16) | (col0g
<< 8),
199 col1
= col
| (col1r
<<16) | (col1g
<< 8);
201 vg_line2( center
, p0
, VG__NONE
, col0
);
202 vg_line2( p0
, p1
, col0
, col1
);
206 v3_muladds( center
, va
, size
, p0
);
207 v3_muladds( p0
, vxb
, size
, p1
);
209 vg_line( p0
, p1
, 0xff0000ff );
211 if( st
->axis_violation
){
212 v3_muladds( p0
, st
->axis_target
, size
*1.25f
, p1
);
213 vg_line( p0
, p1
, 0xffffff00 );
214 vg_line_point( p1
, 0.0025f
, 0xffffff80 );
218 v3_cross( vy
, va
, refaxis
); /* our default rotation */
219 v3_normalize( refaxis
);
221 v3_cross( va
, refaxis
, refaxis_up
);
222 float newang
= acosf(st
->conet
-0.0001f
);
224 v3_muladds( p0
, refaxis_up
, sinf(newang
)*size
, p1
);
225 v3_muladds( p1
, refaxis
, -cosf(newang
)*size
, p1
);
226 vg_line( p0
, p1
, 0xff000000 );
228 v3_muladds( p0
, refaxis_up
, sinf(-newang
)*size
, p1
);
229 v3_muladds( p1
, refaxis
, -cosf(-newang
)*size
, p1
);
230 vg_line( p0
, p1
, 0xff404040 );
235 * Solve a list of positional constraints
237 static void rb_solve_position_constraints( rb_constr_pos
*buf
, int len
){
238 for( int i
=0; i
<len
; i
++ ){
239 rb_constr_pos
*constr
= &buf
[i
];
240 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
243 m3x3_mulv( rba
->to_world
, constr
->lca
, wa
);
244 m3x3_mulv( rbb
->to_world
, constr
->lcb
, wb
);
246 m3x3f ssra
, ssrat
, ssrb
, ssrbt
;
248 m3x3_skew_symetric( ssrat
, wa
);
249 m3x3_skew_symetric( ssrbt
, wb
);
250 m3x3_transpose( ssrat
, ssra
);
251 m3x3_transpose( ssrbt
, ssrb
);
253 v3f b
, b_wa
, b_wb
, b_a
, b_b
;
254 m3x3_mulv( ssra
, rba
->w
, b_wa
);
255 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
256 v3_add( rba
->v
, b_wa
, b
);
257 v3_sub( b
, rbb
->v
, b
);
258 v3_sub( b
, b_wb
, b
);
259 v3_muls( b
, -1.0f
, b
);
262 m3x3_diagonal( invMa
, rba
->inv_mass
);
263 m3x3_diagonal( invMb
, rbb
->inv_mass
);
266 m3x3_mul( ssra
, rba
->iIw
, ia
);
267 m3x3_mul( ia
, ssrat
, ia
);
268 m3x3_mul( ssrb
, rbb
->iIw
, ib
);
269 m3x3_mul( ib
, ssrbt
, ib
);
272 m3x3_add( invMa
, ia
, cma
);
273 m3x3_add( invMb
, ib
, cmb
);
276 m3x3_add( cma
, cmb
, A
);
278 /* Solve Ax = b ( A^-1*b = x ) */
282 m3x3_mulv( invA
, b
, impulse
);
284 v3f delta_va
, delta_wa
, delta_vb
, delta_wb
;
286 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
287 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
289 m3x3_mulv( invMa
, impulse
, delta_va
);
290 m3x3_mulv( invMb
, impulse
, delta_vb
);
291 m3x3_mulv( iwa
, impulse
, delta_wa
);
292 m3x3_mulv( iwb
, impulse
, delta_wb
);
294 v3_add( rba
->v
, delta_va
, rba
->v
);
295 v3_add( rba
->w
, delta_wa
, rba
->w
);
296 v3_sub( rbb
->v
, delta_vb
, rbb
->v
);
297 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
301 static void rb_solve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
303 for( int i
=0; i
<len
; i
++ ){
304 rb_constr_swingtwist
*st
= &buf
[ i
];
306 if( !st
->axis_violation
)
309 float rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
310 v3_dot( st
->axis
, st
->rba
->w
);
312 if( rv
* (float)st
->axis_violation
> 0.0f
)
316 v3_muls( st
->axis
, rv
*st
->axis_mass
, impulse
);
317 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
318 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
320 v3_muls( impulse
, -1.0f
, impulse
);
321 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
322 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
324 float rv2
= v3_dot( st
->axis
, st
->rbb
->w
) -
325 v3_dot( st
->axis
, st
->rba
->w
);
328 for( int i
=0; i
<len
; i
++ ){
329 rb_constr_swingtwist
*st
= &buf
[ i
];
331 if( !st
->tangent_violation
)
334 float rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
335 v3_dot( st
->tangent_axis
, st
->rba
->w
);
341 v3_muls( st
->tangent_axis
, rv
*st
->tangent_mass
, impulse
);
342 m3x3_mulv( st
->rba
->iIw
, impulse
, wa
);
343 v3_add( st
->rba
->w
, wa
, st
->rba
->w
);
345 v3_muls( impulse
, -1.0f
, impulse
);
346 m3x3_mulv( st
->rbb
->iIw
, impulse
, wb
);
347 v3_add( st
->rbb
->w
, wb
, st
->rbb
->w
);
349 float rv2
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
350 v3_dot( st
->tangent_axis
, st
->rba
->w
);
355 static void rb_postsolve_swingtwist_constraints( rb_constr_swingtwist
*buf
,
357 for( int i
=0; i
<len
; i
++ ){
358 rb_constr_swingtwist
*st
= &buf
[ i
];
360 if( !st
->axis_violation
){
361 st
->conv_axis
= 0.0f
;
365 f32 rv
= v3_dot( st
->axis
, st
->rbb
->w
) -
366 v3_dot( st
->axis
, st
->rba
->w
);
368 if( rv
* (f32
)st
->axis_violation
> 0.0f
)
369 st
->conv_axis
= 0.0f
;
374 for( int i
=0; i
<len
; i
++ ){
375 rb_constr_swingtwist
*st
= &buf
[ i
];
377 if( !st
->tangent_violation
){
378 st
->conv_tangent
= 0.0f
;
382 f32 rv
= v3_dot( st
->tangent_axis
, st
->rbb
->w
) -
383 v3_dot( st
->tangent_axis
, st
->rba
->w
);
386 st
->conv_tangent
= 0.0f
;
388 st
->conv_tangent
= rv
;
392 static void rb_solve_constr_angle( rigidbody
*rba
, rigidbody
*rbb
,
394 m3x3f ssra
, ssrb
, ssrat
, ssrbt
;
397 m3x3_skew_symetric( ssrat
, ra
);
398 m3x3_skew_symetric( ssrbt
, rb
);
399 m3x3_transpose( ssrat
, ssra
);
400 m3x3_transpose( ssrbt
, ssrb
);
402 m3x3_mul( ssra
, rba
->iIw
, cma
);
403 m3x3_mul( cma
, ssrat
, cma
);
404 m3x3_mul( ssrb
, rbb
->iIw
, cmb
);
405 m3x3_mul( cmb
, ssrbt
, cmb
);
408 m3x3_add( cma
, cmb
, A
);
412 m3x3_mulv( ssra
, rba
->w
, b_wa
);
413 m3x3_mulv( ssrb
, rbb
->w
, b_wb
);
414 v3_add( b_wa
, b_wb
, b
);
418 m3x3_mulv( invA
, b
, impulse
);
420 v3f delta_wa
, delta_wb
;
422 m3x3_mul( rba
->iIw
, ssrat
, iwa
);
423 m3x3_mul( rbb
->iIw
, ssrbt
, iwb
);
424 m3x3_mulv( iwa
, impulse
, delta_wa
);
425 m3x3_mulv( iwb
, impulse
, delta_wb
);
426 v3_add( rba
->w
, delta_wa
, rba
->w
);
427 v3_sub( rbb
->w
, delta_wb
, rbb
->w
);
431 * Correct position constraint drift errors
432 * [ 0.0 <= amt <= 1.0 ]: the correction amount
434 static void rb_correct_position_constraints( rb_constr_pos
*buf
, int len
,
436 for( int i
=0; i
<len
; i
++ ){
437 rb_constr_pos
*constr
= &buf
[i
];
438 rigidbody
*rba
= constr
->rba
, *rbb
= constr
->rbb
;
441 m3x3_mulv( rba
->to_world
, constr
->lca
, p0
);
442 m3x3_mulv( rbb
->to_world
, constr
->lcb
, p1
);
443 v3_add( rba
->co
, p0
, p0
);
444 v3_add( rbb
->co
, p1
, p1
);
448 v3_muladds( rbb
->co
, d
, -1.0f
* amt
, rbb
->co
);
449 rb_update_matrices( rbb
);
451 f32 mt
= 1.0f
/(rba
->inv_mass
+rbb
->inv_mass
),
452 a
= mt
* (k_phys_baumgarte
/k_rb_delta
);
454 v3_muladds( rba
->v
, d
, a
* rba
->inv_mass
, rba
->v
);
455 v3_muladds( rbb
->v
, d
, a
*-rbb
->inv_mass
, rbb
->v
);
460 static void rb_correct_swingtwist_constraints( rb_constr_swingtwist
*buf
,
461 int len
, float amt
){
462 for( int i
=0; i
<len
; i
++ ){
463 rb_constr_swingtwist
*st
= &buf
[i
];
465 if( !st
->tangent_violation
)
469 m3x3_mulv( st
->rbb
->to_world
, st
->coneva
, va
);
471 f32 angle
= v3_dot( va
, st
->tangent_target
);
473 if( fabsf(angle
) < 0.9999f
){
475 v3_cross( va
, st
->tangent_target
, axis
);
477 angle
= acosf(angle
) * amt
;
479 q_axis_angle( correction
, axis
, angle
);
480 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
481 q_normalize( st
->rbb
->q
);
482 rb_update_matrices( st
->rbb
);
484 f32 mt
= 1.0f
/(st
->rba
->inv_mass
+st
->rbb
->inv_mass
),
485 wa
= mt
* acosf(angle
) * (k_phys_baumgarte
/k_rb_delta
);
486 //v3_muladds( st->rba->w, axis, wa*-st->rba->inv_mass, st->rba->w );
487 v3_muladds( st
->rbb
->w
, axis
, wa
* st
->rbb
->inv_mass
, st
->rbb
->w
);
492 for( int i
=0; i
<len
; i
++ ){
493 rb_constr_swingtwist
*st
= &buf
[i
];
495 if( !st
->axis_violation
)
499 m3x3_mulv( st
->rbb
->to_world
, st
->conevxb
, vxb
);
501 f32 angle
= v3_dot( vxb
, st
->axis_target
);
503 if( fabsf(angle
) < 0.9999f
){
505 v3_cross( vxb
, st
->axis_target
, axis
);
508 angle
= acosf(angle
) * amt
;
510 q_axis_angle( correction
, axis
, angle
);
511 q_mul( correction
, st
->rbb
->q
, st
->rbb
->q
);
512 q_normalize( st
->rbb
->q
);
513 rb_update_matrices( st
->rbb
);
515 f32 mt
= 1.0f
/(st
->rba
->inv_mass
+st
->rbb
->inv_mass
),
516 wa
= mt
* acosf(angle
) * (k_phys_baumgarte
/k_rb_delta
);
517 //v3_muladds( st->rba->w, axis, wa*-0.5f, st->rba->w );
518 v3_muladds( st
->rbb
->w
, axis
, wa
* st
->rbb
->inv_mass
, st
->rbb
->w
);