From 33615ea88d1d44a04252347fdf28983116a365e0 Mon Sep 17 00:00:00 2001 From: Cyrille Henry Date: Sun, 26 Jul 2015 20:08:01 +0000 Subject: add a next type of like : hinge can force an angle between masses... svn path=/trunk/externals/pmpd/; revision=17500 --- examples/68_2d_hinge.pd | 107 ++++++++++++++++++++++++++++++++++++ pmpd-meta.pd | 2 +- pmpd2d.c | 1 + pmpd2d.h | 11 ++-- pmpd2d_core.c | 141 ++++++++++++++++++++++++++++++++++++++++++++++-- pmpd2d_set.c | 1 - pmpd2d_various.c | 7 ++- 7 files changed, 259 insertions(+), 11 deletions(-) create mode 100644 examples/68_2d_hinge.pd diff --git a/examples/68_2d_hinge.pd b/examples/68_2d_hinge.pd new file mode 100644 index 0000000..a901298 --- /dev/null +++ b/examples/68_2d_hinge.pd @@ -0,0 +1,107 @@ +#N canvas 640 304 796 716 10; +#X obj 31 576 pmpd2d; +#X msg 99 332 setK hinge \$1; +#X msg 129 384 setD hinge \$1; +#X floatatom 99 308 5 0 0 0 - - -, f 5; +#X floatatom 129 361 5 0 0 0 - - -, f 5; +#N canvas 986 544 784 467 gemrender 0; +#X obj 48 243 translateXYZ; +#X obj 48 269 sphere 0.1; +#X obj 127 24 inlet; +#X obj 559 12 inlet; +#X obj 475 232 curve 2; +#X msg 631 81 \$4 \$5 0; +#X msg 559 77 \$2 \$3 0; +#X obj 559 105 t b a; +#X obj 127 102 unpack f f f; +#X obj 166 128 t b f; +#X msg 48 150 0; +#X obj 48 117 loadbang; +#X obj 48 177 gemhead; +#X msg 475 160 0; +#X obj 475 127 loadbang; +#X obj 475 187 gemhead; +#X obj 559 42 t a a; +#X obj 127 45 route fix mvt; +#X obj 235 233 translateXYZ; +#X obj 235 259 sphere 0.1; +#X obj 314 92 unpack f f f; +#X obj 353 118 t b f; +#X msg 235 140 0; +#X obj 235 107 loadbang; +#X obj 235 167 gemhead; +#X obj 48 211 color 1 0 0; +#X connect 0 0 1 0; +#X connect 2 0 17 0; +#X connect 3 0 16 0; +#X connect 5 0 4 2; +#X connect 6 0 7 0; +#X connect 7 0 15 0; +#X connect 7 1 4 1; +#X connect 8 1 9 0; +#X connect 8 2 0 2; +#X connect 9 0 12 0; +#X connect 9 1 0 1; +#X connect 10 0 12 0; +#X connect 11 0 10 0; +#X connect 12 0 25 0; +#X connect 13 0 15 0; +#X connect 14 0 13 0; +#X connect 15 0 4 0; +#X connect 16 0 6 0; +#X connect 16 1 5 0; +#X connect 17 0 8 0; +#X connect 17 1 20 0; +#X connect 18 0 19 0; +#X connect 20 1 21 0; +#X connect 20 2 18 2; +#X connect 21 0 24 0; +#X connect 21 1 18 1; +#X connect 22 0 24 0; +#X connect 23 0 22 0; +#X connect 24 0 18 0; +#X connect 25 0 0 0; +#X restore 31 630 pd gemrender; +#X obj 154 628 print; +#X msg 42 47 0 \, destroy; +#X msg 32 23 reset \, create \, 1; +#X msg 229 446 setL hinge \$1; +#X floatatom 229 424 5 0 0 0 - - -, f 5; +#X msg 31 146 reset \, mass fix 0 1 -1 0 \, mass fix 0 1 1 0 \, mass +mvt 1 1 3 1 \, hinge hinge 0 1 2 0.1 0.1 \, link L 1 0 0.5 0.3 \, link +L 1 2 0.5 0.3; +#X msg 42 195 reset \, mass fix 0 1 -1 0 \, mass mvt 1 1 1 0 \, mass +mvt 1 1 3 1 \, hinge hinge 0 1 2 0.1 0.1 \, link L 1 0 0.5 0.3 \, link +L 1 2 0.5 0.3; +#X msg 56 244 reset \, mass mvt 1 1 -1 0 \, mass mvt 1 1 1 0 \, mass +mvt 1 1 3 1 \, hinge hinge 0 1 2 0.1 0.1 \, link L 1 0 0.5 0.3 \, link +L 1 2 0.5 0.3; +#X msg 46 550 bang \, massPos \, linkEnd L; +#X obj 31 604 route massPos linkEnd; +#X obj 32 69 gemwin 50; +#X obj 31 123 loadbang; +#X text 197 16 hing links allow to connect 3 masses in order to force +an angle between them; +#X text 410 157 1 moving mass; +#X text 429 205 2 moving masses; +#X text 445 257 3 moving masses; +#X obj 46 530 gemhead 66; +#X text 271 425 angle (in radian); +#X connect 0 0 15 0; +#X connect 1 0 0 0; +#X connect 2 0 0 0; +#X connect 3 0 1 0; +#X connect 4 0 2 0; +#X connect 7 0 16 0; +#X connect 8 0 16 0; +#X connect 9 0 0 0; +#X connect 10 0 9 0; +#X connect 11 0 0 0; +#X connect 12 0 0 0; +#X connect 13 0 0 0; +#X connect 14 0 0 0; +#X connect 15 0 5 0; +#X connect 15 1 5 1; +#X connect 15 2 6 0; +#X connect 17 0 11 0; +#X connect 22 0 14 0; diff --git a/pmpd-meta.pd b/pmpd-meta.pd index c0c88f2..ada0852 100644 --- a/pmpd-meta.pd +++ b/pmpd-meta.pd @@ -4,5 +4,5 @@ #X text 10 58 DESCRIPTION Pmpd is a library for Pure Data allowing the synthesis of control data through the means of particular physics modelling.; -#X text 9 135 VERSION 0.10-svn; +#X text 9 135 VERSION 0.11-svn; #X text 10 38 AUTHOR Cyrille Henry and Nicolas Montgermont; diff --git a/pmpd2d.c b/pmpd2d.c index db61e8b..a00738e 100644 --- a/pmpd2d.c +++ b/pmpd2d.c @@ -59,6 +59,7 @@ pmpd2d_class = class_new(gensym("pmpd2d"), class_addmethod(pmpd2d_class, (t_method)pmpd2d_reset, gensym("reset"), 0); class_addmethod(pmpd2d_class, (t_method)pmpd2d_mass, gensym("mass"), A_GIMME, 0); class_addmethod(pmpd2d_class, (t_method)pmpd2d_link, gensym("link"), A_GIMME, 0); + class_addmethod(pmpd2d_class, (t_method)pmpd2d_hinge, gensym("hinge"), A_GIMME, 0); class_addmethod(pmpd2d_class, (t_method)pmpd2d_tLink, gensym("tLink"), A_GIMME, 0); class_addmethod(pmpd2d_class, (t_method)pmpd2d_tabLink, gensym("tabLink"), A_GIMME, 0); class_addmethod(pmpd2d_class, (t_method)pmpd2d_delLink, gensym("delLink"), A_GIMME, 0); diff --git a/pmpd2d.h b/pmpd2d.h index 2ab71df..1cd3da8 100644 --- a/pmpd2d.h +++ b/pmpd2d.h @@ -24,17 +24,18 @@ typedef struct _mass { typedef struct _link { t_symbol *Id; - int lType; + int lType; // 0 pour un lien normal, 1 pour un tLink, 2 pour un tabLink, 3 pour un hinge struct _mass *mass1; struct _mass *mass2; + struct _mass *mass3; // seulement pour le hinge t_int active; t_float K; t_float D; - t_float L; + t_float L; // teta t_float Pow; - t_float Lmin; - t_float Lmax; - t_float distance; + t_float Lmin; // tetamin pour un hinge + t_float Lmax; // tetamax + t_float distance; // angle de la charniere ds le cas d'un hinge t_float VX; // vecteur portant la liaison, si c'est le cas t_float VY; t_symbol *arrayK; diff --git a/pmpd2d_core.c b/pmpd2d_core.c index 22c8f03..eeebafc 100644 --- a/pmpd2d_core.c +++ b/pmpd2d_core.c @@ -45,6 +45,50 @@ t_float tabread2(t_pmpd2d *x, t_float pos, t_symbol *array) return( pos); // si il y a un pb sur le tableau, on renvoie l'identité } +t_float pow2(t_float x) +{ + return(x*x); +} + +t_float getAngle_bug(t_pmpd2d *x, t_int mass1, t_int mass3, t_int mass2) +{ + t_float X12, X13, X23, Y12, Y13, Y23, D; + X12 = x->mass[mass1].posX - x->mass[mass2].posX; + Y12 = x->mass[mass1].posY - x->mass[mass2].posY; + X13 = x->mass[mass1].posX - x->mass[mass3].posX; + Y13 = x->mass[mass1].posY - x->mass[mass3].posY; + X23 = x->mass[mass2].posX - x->mass[mass3].posX; + Y23 = x->mass[mass2].posY - x->mass[mass3].posY; + + D = sqrt(X13*X13 + Y13*Y13) + sqrt(X23*X23 + Y23*Y23); + if (D =! 0) + return(acos((X13*X23 + Y13*Y23)/D)); + else + return(0); +} + +t_float getAngle(t_pmpd2d *x, t_int mass1, t_int mass2, t_int mass3) +{ + t_float A1, A2; + A1 = atan2( x->mass[mass1].posX - x->mass[mass2].posX, x->mass[mass1].posY - x->mass[mass2].posY); + A2 = atan2( x->mass[mass3].posX - x->mass[mass2].posX, x->mass[mass3].posY - x->mass[mass2].posY); + return(A2-A1); +} + +t_float distance(t_pmpd2d *x, t_int mass1, t_int mass2) +{ + t_float X,Y; + X = x->mass[mass1].posX - x->mass[mass2].posX; + Y = x->mass[mass1].posY - x->mass[mass2].posY; + return(sqrt(X*X+Y*Y)); +} + +t_float mod2Pi(t_float angle) +{ + t_float tmp; + tmp = fmodf(angle-3.1415926, 6.2831852); + return(tmp > 0 ? tmp-3.1415926 : tmp+3.1415926); // TODO : mettre un vrai PI +} void pmpd2d_reset(t_pmpd2d *x) { x->nb_link = 0; @@ -70,10 +114,11 @@ void *pmpd2d_new() void pmpd2d_bang(t_pmpd2d *x) { // this part is doing all the PM - t_float F, L, Lx,Ly, Fx, Fy, tmp, tmpX, tmpY,speed; + t_float F, L, Dist, Lx,Ly, Fx, Fy, tmp, tmpX, tmpY,speed; t_int i; for (i=0; inb_mass; i++) // compute new masses position + { if (x->mass[i].mobile > 0) // only if mobile { // amplify force that opose to movement @@ -121,9 +166,13 @@ void pmpd2d_bang(t_pmpd2d *x) x->mass[i].forceY += x->mass[i].D2offset * (x->mass[i].speedY/speed); } } - + } for (i=0; inb_link; i++) // compute link forces - if (x->link[i].active > 0) + { + x->link[i].forceX = 0; + x->link[i].forceY = 0; + + if ((x->link[i].active > 0) && (x->link[i].lType < 3)) { Lx = x->link[i].mass1->posX - x->link[i].mass2->posX; Ly = x->link[i].mass1->posY - x->link[i].mass2->posY; @@ -161,6 +210,40 @@ void pmpd2d_bang(t_pmpd2d *x) } x->link[i].distance=L; } + else if ((x->link[i].active > 0) && (x->link[i].lType == 3)) // hinge + { + L = getAngle(x, x->link[i].mass1->num, x->link[i].mass2->num, x->link[i].mass3->num); + L = mod2Pi(L); + if ( (L >= x->link[i].Lmin) && (L < x->link[i].Lmax) && (L != 0)) + { + F = x->link[i].D * (mod2Pi(L - x->link[i].distance)); + F += x->link[i].K * pow_ch( mod2Pi(L - x->link[i].L), x->link[i].Pow); // calcule du couple + + // post("f=%f", mod2Pi(L - x->link[i].L)); + Dist = distance(x, x->link[i].mass1->num, x->link[i].mass2->num); + tmp = F/Dist; // couple pour la mass 1 + Fx = tmp * (x->link[i].mass1->posY - x->link[i].mass2->posY)/Dist; + Fy = tmp * (x->link[i].mass2->posX - x->link[i].mass1->posX)/Dist; // projection sur la normal a la liaison mass1 mass2 + x->link[i].mass1->forceX += Fx; + x->link[i].mass1->forceY += Fy; + x->link[i].mass2->forceX -= Fx; + x->link[i].mass2->forceY -= Fy; + + x->link[i].forceX = Fx; // save for latter use + x->link[i].forceY = Fy; // que doit on faire ds ce cas la : quel est la valeur a sauver ds la cas d'un couple??? + + Dist = distance(x, x->link[i].mass3->num, x->link[i].mass2->num); + F /= Dist;// pour la mass 2 + Fx = F * (x->link[i].mass3->posY - x->link[i].mass2->posY)/Dist; + Fy = F * (x->link[i].mass2->posX - x->link[i].mass3->posX)/Dist; // projection sur la normal a la liaison mass2 mass3 + x->link[i].mass3->forceX -= Fx; + x->link[i].mass3->forceY -= Fy; + x->link[i].mass2->forceX += Fx; + x->link[i].mass2->forceY += Fy; + } + x->link[i].distance=L; // sauvegarde de l'angle de la liaison + } + } } void pmpd2d_mass(t_pmpd2d *x, t_symbol *s, int argc, t_atom *argv) @@ -437,6 +520,58 @@ void pmpd2d_tabLink(t_pmpd2d *x, t_symbol *s, int argc, t_atom *argv) } } +void pmpd2d_hinge(t_pmpd2d *x, t_symbol *s, int argc, t_atom *argv) +{ // TODO : acept symbol ID for hinge creation + int mass1, mass2, mass3; + + x->link[x->nb_link].Id = gensym("hinge"); + if ((argc > 0) && (argv[0].a_type == A_SYMBOL)) + x->link[x->nb_link].Id = atom_getsymbolarg(0,argc,argv); + + mass1 = 0; + if ((argc > 1) && (argv[1].a_type == A_FLOAT)) + mass1 = atom_getfloatarg(1, argc, argv); + + mass2 = 0; + if ((argc > 2) && (argv[2].a_type == A_FLOAT)) + mass2 = atom_getfloatarg(2, argc, argv); + + mass3 = 0; + if ((argc > 3) && (argv[3].a_type == A_FLOAT)) + mass3 = atom_getfloatarg(3, argc, argv); + + x->link[x->nb_link].K = 0; + if ((argc > 4) && (argv[4].a_type == A_FLOAT)) + x->link[x->nb_link].K = atom_getfloatarg(4, argc, argv); + + x->link[x->nb_link].D = 0; + if ((argc > 5) && (argv[5].a_type == A_FLOAT)) + x->link[x->nb_link].D = atom_getfloatarg(5, argc, argv); + + x->link[x->nb_link].active = 1; + x->link[x->nb_link].Pow = 1; + x->link[x->nb_link].Lmin = -4; + x->link[x->nb_link].Lmax = 4; + + + if ( (mass1 != mass2) && (mass2 != mass3) && (mass1 != mass3) \ + && (mass1 < x->nb_mass) && (mass2 < x->nb_mass) && (mass3 < x->nb_mass)) + { + x->link[x->nb_link].mass1 = &x->mass[mass1]; + x->link[x->nb_link].mass2 = &x->mass[mass2]; + x->link[x->nb_link].mass3 = &x->mass[mass3]; + + x->link[x->nb_link].L = getAngle(x, mass1, mass2, mass3); // angle actuel : -pi a pi + + x->link[x->nb_link].distance = x->link[x->nb_link].L; // angle au repos = angle a l'initialisation + + x->link[x->nb_link].lType = 3; + + x->nb_link++ ; + x->nb_link = min (nb_max_link -1, x->nb_link ); + } +} + void pmpd2d_delLink_int(t_pmpd2d *x, int dellink) { int i; diff --git a/pmpd2d_set.c b/pmpd2d_set.c index a74a7ca..f6807cd 100644 --- a/pmpd2d_set.c +++ b/pmpd2d_set.c @@ -152,7 +152,6 @@ void pmpd2d_setD(t_pmpd2d *x, t_symbol *s, int argc, t_atom *argv) } } - void pmpd2d_setPow(t_pmpd2d *x, t_symbol *s, int argc, t_atom *argv) { int tmp, end, i, offset; diff --git a/pmpd2d_various.c b/pmpd2d_various.c index 9c54b7f..6ebbf85 100644 --- a/pmpd2d_various.c +++ b/pmpd2d_various.c @@ -11,7 +11,7 @@ void pmpd2d_infosL(t_pmpd2d *x) } post("list of %d link", x->nb_link); - post("number, Id, active, mass1, mass2, K, D, Pow, L, Lmin, Lmax"); + post("number, Id, active, mass1, mass2, [mass3], K, D, Pow, L, Lmin, Lmax"); for(i=0; i < x->nb_link; i++) { switch(x->link[i].lType) @@ -31,6 +31,11 @@ void pmpd2d_infosL(t_pmpd2d *x) x->link[i].mass1->num, x->link[i].mass2->num, x->link[i].K, x->link[i].D, \ x->link[i].arrayK->s_name, x->link[i].K_L, x->link[i].arrayD->s_name, x->link[i].D_L); break; + case 3 : + post("hinge %i: %s, %i, %i, %i, %i, %f, %f, %f, %f, %f, %f, %f",i, x->link[i].Id->s_name, x->link[i].active, \ + x->link[i].mass1->num, x->link[i].mass2->num, x->link[i].mass3->num, x->link[i].K, x->link[i].D, \ + x->link[i].Pow, x->link[i].L, x->link[i].distance, x->link[i].Lmin, x->link[i].Lmax); + break; } } } -- cgit v1.2.1