add a next type of like : hinge can force an angle between masses...

svn path=/trunk/externals/pmpd/; revision=17500

7 files changed, 259 insertions, 11 deletions

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; i<x->nb_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; i<x->nb_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;
         }
     }
 }