Initial revision
[blender.git] / source / blender / blenkernel / intern / ika.c
1
2 /*  ika.c      MIXED MODEL
3  * 
4  *  april 96
5  *  
6  * 
7  * $Id$
8  *
9  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
10  *
11  * This program is free software; you can redistribute it and/or
12  * modify it under the terms of the GNU General Public License
13  * as published by the Free Software Foundation; either version 2
14  * of the License, or (at your option) any later version. The Blender
15  * Foundation also sells licenses for use in proprietary software under
16  * the Blender License.  See http://www.blender.org/BL/ for information
17  * about this.
18  *
19  * This program is distributed in the hope that it will be useful,
20  * but WITHOUT ANY WARRANTY; without even the implied warranty of
21  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
22  * GNU General Public License for more details.
23  *
24  * You should have received a copy of the GNU General Public License
25  * along with this program; if not, write to the Free Software Foundation,
26  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
27  *
28  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
29  * All rights reserved.
30  *
31  * The Original Code is: all of this file.
32  *
33  * Contributor(s): none yet.
34  *
35  * ***** END GPL/BL DUAL LICENSE BLOCK *****
36  */
37
38 #include <math.h>
39 #include <stdlib.h>
40
41 #include "MEM_guardedalloc.h"
42
43 /* types */
44 #include "DNA_ika_types.h"
45 #include "DNA_object_types.h"
46 #include "DNA_scene_types.h"
47 #include "DNA_view3d_types.h"
48
49 #include "BLI_blenlib.h"
50 #include "BLI_arithb.h"
51 /* functions */
52 #include "BKE_blender.h"
53 #include "BKE_library.h"
54 #include "BKE_global.h"
55 #include "BKE_main.h"
56 #include "BKE_object.h"
57 #include "BKE_ika.h"
58
59 /* Let's go! */
60 #define TOLER 0.000076
61 #define CLAMP(a, b, c) if((a)<(b)) (a)=(b); else if((a)>(c)) (a)=(c) 
62
63
64 void unlink_ika(Ika *ika)
65 {
66         /* loskoppelen: */
67         
68
69 }
70
71 /* niet Ika zelf vrijgeven */
72 void free_ika(Ika *ika)
73 {
74
75         /* unimplemented!!! */
76         unlink_ika(ika); 
77         
78         BLI_freelistN(&ika->limbbase);
79         
80         if(ika->def) MEM_freeN(ika->def);
81 }
82
83 Ika *add_ika()
84 {
85         Ika *ika;
86         
87         ika= alloc_libblock(&G.main->ika, ID_IK, "Ika");
88         ika->flag = IK_GRABEFF | IK_XYCONSTRAINT;
89
90         ika->xyconstraint= 0.5f;
91         ika->mem= 0.3f;
92         ika->iter= 6;
93         
94         return ika;
95 }
96
97 Ika *copy_ika(Ika *ika)
98 {
99         Ika *ikan;
100         
101         ikan= copy_libblock(ika);
102         
103         duplicatelist(&ikan->limbbase, &ika->limbbase);
104
105         ikan->def= MEM_dupallocN(ikan->def);
106         
107         return ikan;
108 }
109
110 void make_local_ika(Ika *ika)
111 {
112         Object *ob;
113         Ika *ikan;
114         int local=0, lib=0;
115         
116         /* - zijn er alleen lib users: niet doen
117          * - zijn er alleen locale users: flag zetten
118          * - mixed: copy
119          */
120         
121         if(ika->id.lib==0) return;
122         if(ika->id.us==1) {
123                 ika->id.lib= 0;
124                 ika->id.flag= LIB_LOCAL;
125                 new_id(0, (ID *)ika, 0);
126                 return;
127         }
128         
129         ob= G.main->object.first;
130         while(ob) {
131                 if(ob->data==ika) {
132                         if(ob->id.lib) lib= 1;
133                         else local= 1;
134                 }
135                 ob= ob->id.next;
136         }
137         
138         if(local && lib==0) {
139                 ika->id.lib= 0;
140                 ika->id.flag= LIB_LOCAL;
141                 new_id(0, (ID *)ika, 0);
142         }
143         else if(local && lib) {
144                 ikan= copy_ika(ika);
145                 ikan->id.us= 0;
146                 
147                 ob= G.main->object.first;
148                 while(ob) {
149                         if(ob->data==ika) {
150                                 
151                                 if(ob->id.lib==0) {
152                                         ob->data= ikan;
153                                         ikan->id.us++;
154                                         ika->id.us--;
155                                 }
156                         }
157                         ob= ob->id.next;
158                 }
159         }
160 }
161
162 int count_limbs(Object *ob)
163 {
164         int tot=0;
165         Ika *ika;
166         Limb *li;
167         
168         if(ob->type!=OB_IKA) return 0;
169         ika= ob->data;
170         
171         li= ika->limbbase.first;
172         while(li) {
173                 tot++;
174                 li= li->next;
175         }
176         return tot;
177 }
178
179 /* ************************************************** */
180
181
182 /* aan hand van eff[] de len en alpha */
183 void calc_limb(Limb *li)
184 {
185         Limb *prev= li;
186         float vec[2], alpha= 0.0;
187         
188         /* alpha van 'parents' */
189         while( (prev=prev->prev) ) {
190           alpha+= prev->alpha;
191         }
192         
193         if(li->prev) {
194                 vec[0]= -li->prev->eff[0];
195                 vec[1]= -li->prev->eff[1];
196         }
197         else vec[0]= vec[1]= 0.0;
198         
199         vec[0]+= li->eff[0];
200         vec[1]+= li->eff[1];
201
202         li->alpha= (float)atan2(vec[1], vec[0]) - alpha;
203         li->len= (float)sqrt(vec[0]*vec[0] + vec[1]*vec[1]);
204
205 }
206
207 /* aan hand van len en alpha worden de eindpunten berekend */
208 void calc_ika(Ika *ika, Limb *li)
209 {
210         float alpha=0.0, co, si;
211
212         if(li) {                
213                 Limb *prev= li;
214                 while((prev=prev->prev)) {
215                   alpha+= prev->alpha;
216                 }
217         }
218         else li= ika->limbbase.first;
219
220         while(li) {
221                 if(li->alpha != li->alpha) li->alpha= 0.0f;     /* NaN patch */
222
223                 alpha+= li->alpha;
224
225                 co= (float)cos(alpha);
226                 si= (float)sin(alpha);
227                 
228                 li->eff[0]= co*li->len;
229                 li->eff[1]= si*li->len;
230                 
231                 if(li->prev) {
232                         li->eff[0]+= li->prev->eff[0];
233                         li->eff[1]+= li->prev->eff[1];
234                 }
235                 
236                 if(li->next==0) {
237                         ika->eff[0]= li->eff[0];
238                         ika->eff[1]= li->eff[1];
239                 }
240                 
241                 li= li->next;
242         }
243 }
244
245 void init_defstate_ika(Object *ob)
246 {
247         Ika *ika;
248         Limb *li;
249         
250         ika= ob->data;
251         ika->totx= 0.0;
252         ika->toty= 0.0;
253         li= ika->limbbase.first;
254         
255         calc_ika(ika, 0);       /* correcte eindpunten */
256         
257         while(li) {
258                 li->alphao= li->alpha;
259                 li->leno= li->len;
260                 
261                 li= li->next;
262         }
263         ika->eff[2]= 0.0;
264         VecMat4MulVecfl(ika->effg, ob->obmat, ika->eff);
265 }
266
267 void itterate_limb(Ika *ika, Limb *li)
268 {
269         float da, n1[2], n2[2], len1, len2;
270         
271         if(li->prev) {
272                 n1[0]= ika->eff[0] - li->prev->eff[0];
273                 n1[1]= ika->eff[1] - li->prev->eff[1];
274                 n2[0]= ika->effn[0] - li->prev->eff[0];
275                 n2[1]= ika->effn[1] - li->prev->eff[1];
276         }
277         else {
278                 n1[0]= ika->eff[0];
279                 n1[1]= ika->eff[1];
280                 n2[0]= ika->effn[0];
281                 n2[1]= ika->effn[1];
282         }
283         len1= (float)sqrt(n1[0]*n1[0] + n1[1]*n1[1]);
284         len2= (float)sqrt(n2[0]*n2[0] + n2[1]*n2[1]);
285
286         da= (1.0f-li->fac)*saacos( (n1[0]*n2[0]+n1[1]*n2[1])/(len1*len2) );
287
288         if(n1[0]*n2[1] < n1[1]*n2[0]) da= -da;
289         
290         li->alpha+= da;
291         
292 }
293
294 void rotate_ika(Object *ob, Ika *ika)
295 {
296         Limb *li;
297         float len2, da, n1[2], n2[2];
298         
299         /* terug roteren */
300         euler_rot(ob->rot, -ika->toty, 'y');
301         ika->toty= 0.0;
302         
303         where_is_object(ob);
304         
305         Mat4Invert(ob->imat, ob->obmat);
306         VecMat4MulVecfl(ika->effn, ob->imat, ika->effg);
307         
308         li= ika->limbbase.last;
309         if(li==0) return;
310         
311         n1[0]= ika->eff[0];
312         n2[0]= ika->effn[0];
313         n2[1]= ika->effn[2];
314         
315         len2= (float)sqrt(n2[0]*n2[0] + n2[1]*n2[1]);
316         
317         if(len2>TOLER) {
318                 da= (n2[0])/(len2);
319                 if(n1[0]<0.0) da= -da;
320                 
321                 /* als de x comp bijna nul is kan dit gebeuren */
322                 if(da<=-1.0+TOLER || da>=1.0) ;
323                 else {
324                 
325                         da= saacos( da );
326                         if(n1[0]*n2[1] > 0.0) da= -da;
327         
328                         euler_rot(ob->rot, da, 'y');
329                         ika->toty= da;
330                 }
331         }
332 }
333
334 void rotate_ika_xy(Object *ob, Ika *ika)
335 {
336         Limb *li;
337         float ang, da, n1[3], n2[3], axis[3], quat[4];
338         
339         /* terug roteren */
340         euler_rot(ob->rot, -ika->toty, 'y');
341         euler_rot(ob->rot, -ika->totx, 'x');
342         
343         where_is_object(ob);
344
345         Mat4Invert(ob->imat, ob->obmat);
346         VecMat4MulVecfl(ika->effn, ob->imat, ika->effg);
347         
348         li= ika->limbbase.last;
349         if(li==0) return;
350         
351         /* ika->eff = old situation */
352         /* ika->effn = desired situation */
353         
354         *(n1)= *(ika->effn);
355         *(n1+1)= *(ika->effn+1);
356         *(n1+2)= 0.0;
357
358         *(n2)= *(ika->effn);
359         *(n2+1)= *(ika->effn+1);
360         *(n2+2)= *(ika->effn+2);
361         
362         Normalise(n1);
363         Normalise(n2);
364
365         ang= n1[0]*n2[0] + n1[1]*n2[1] + n1[2]*n2[2];
366         ang= saacos(ang);
367                 
368         if(ang<-0.0000001 || ang>0.00000001) {
369                 Crossf(axis, n1, n2);
370                 Normalise(axis);
371                 quat[0]= (float)cos(0.5*ang);
372                 da= (float)sin(0.5*ang);
373                 quat[1]= da*axis[0];
374                 quat[2]= da*axis[1];
375                 quat[3]= da*axis[2];
376         
377                 QuatToEul(quat, axis);
378
379                 ika->totx= axis[0];
380                 CLAMP(ika->totx, -ika->xyconstraint, ika->xyconstraint);
381                 ika->toty= axis[1];
382                 CLAMP(ika->toty, -ika->xyconstraint, ika->xyconstraint);
383         }
384
385         euler_rot(ob->rot, ika->totx, 'x');
386         euler_rot(ob->rot, ika->toty, 'y');
387 }
388
389 void itterate_ika(Object *ob)
390 {
391         Ika *ika;
392         Limb *li;
393         int it = 0;
394
395         ika= ob->data;
396         if((ika->flag & IK_GRABEFF)==0) return;
397
398         disable_where_script(1);
399         /* memory: grote tijdsprongen afvangen */
400         it= abs(ika->lastfra - G.scene->r.cfra);
401         ika->lastfra= G.scene->r.cfra;
402         if(it>10) {
403                 
404                 /* one itteration extra */
405                 itterate_ika(ob);
406         }
407         else {
408                 li= ika->limbbase.first;
409                 while(li) {
410                         li->alpha= (1.0f-ika->mem)*li->alpha + ika->mem*li->alphao;
411                         if(li->fac==1.0f) li->fac= 0.05f;       /* oude files: kan weg in juni 96 */
412                         li= li->next;
413                 }
414         }
415         calc_ika(ika, 0);
416         
417         /* effector heeft parent? */
418         if(ika->parent) {
419                 
420                 if(ika->partype==PAROBJECT) {
421                         if(ika->parent->ctime != (float) G.scene->r.cfra) where_is_object(ika->parent);
422                         *(ika->effg)= *(ika->parent->obmat[3]);
423                         *(ika->effg+1)= *(ika->parent->obmat[3]+1);
424                         *(ika->effg+2)= *(ika->parent->obmat[3]+2);                     
425                 }
426                 else {
427                         what_does_parent1(ika->parent, ika->partype, ika->par1, 0, 0);
428                         *(ika->effg)= *(workob.obmat[3]);
429                         *(ika->effg+1)= *(workob.obmat[3]+1);
430                         *(ika->effg+2)= *(workob.obmat[3]+2);                   
431                 }
432         }
433
434
435         /* y-as goed draaien */
436         if(ika->flag & IK_XYCONSTRAINT) 
437                 rotate_ika_xy(ob, ika);
438         else
439                 rotate_ika(ob, ika);
440
441         it= ika->iter;
442         while(it--) {
443                 
444                 where_is_object(ob);
445                 Mat4Invert(ob->imat, ob->obmat);
446                 VecMat4MulVecfl(ika->effn, ob->imat, ika->effg);
447                 /* forward: dan gaan ook de eerste limbs */
448                 li= ika->limbbase.first;
449                 while(li) {
450                         
451                         itterate_limb(ika, li);
452                         
453                         /* zet je calc_ika() buiten deze lus: lange kettingen instabiel */
454                         calc_ika(ika, li);
455
456                         li= li->next;
457                 }
458
459                 where_is_object(ob);
460                 Mat4Invert(ob->imat, ob->obmat);
461                 VecMat4MulVecfl(ika->effn, ob->imat, ika->effg);
462
463                 /* backward */
464                 li= ika->limbbase.last;
465                 while(li) {
466                         
467                         itterate_limb(ika, li);
468                         
469                         /* zet je calc_ika() buiten deze lus: lange kettingen instabiel */
470                         calc_ika(ika, li);
471
472                         li= li->prev;
473                 }
474         }
475
476         disable_where_script(0);
477 }
478
479
480 void do_all_ikas()
481 {
482         Base *base = 0;
483         
484         base= G.scene->base.first;
485         while(base) {
486                 
487                 if(base->object->type==OB_IKA) itterate_ika(base->object);
488
489                 base= base->next;
490         }
491 }
492
493 void do_all_visible_ikas()
494 {
495         Base *base = 0;
496         
497         base= G.scene->base.first;
498         while(base) {
499                 if(base->lay & G.scene->lay) {
500                         if(base->object->type==OB_IKA) itterate_ika(base->object);
501                 }
502                 base= base->next;
503         }
504 }
505
506 /* ******************** DEFORM ************************ */
507
508
509 void init_skel_deform(Object *par, Object *ob)
510 {
511         Deform *def;
512         Ika *ika;
513         int a;
514         
515         /*  deform:
516          * 
517          *  ob_vec * ob_obmat * def_imat (weight fie) * def_obmat * ob_imat = ob_vec'
518          *   
519          *           <----- premat ---->                <---- postmat ---->
520          */
521         
522         if(par->type!=OB_IKA) return;
523         
524         Mat4Invert(ob->imat, ob->obmat);
525
526         ika= par->data;
527         a= ika->totdef;
528         def= ika->def;
529         while(a--) {
530                 
531                 what_does_parent1(def->ob, def->partype, def->par1, def->par2, def->par3);
532                 
533                 Mat4MulMat4(def->premat, ob->obmat, def->imat);
534                 Mat4MulMat4(def->postmat, workob.obmat, ob->imat);
535
536                 def++;
537         }
538 }
539
540
541 void calc_skel_deform(Ika *ika, float *co)
542 {
543         Deform *def;
544         int a;
545         float totw=0.0, weight, fac, len, vec[3], totvec[3];
546         
547         def= ika->def;
548         if(def==0) return;
549         a= ika->totdef;
550         totvec[0]=totvec[1]=totvec[2]= 0.0;
551         
552         while(a--) {
553                 
554                 VecMat4MulVecfl(vec, def->premat, co);
555                 
556                 len= (float)sqrt(vec[0]*vec[0] + vec[1]*vec[1] + vec[2]*vec[2]);
557                 
558                 if(def->vec[0]==0.0f) len= 2.0f*len;
559                 else len= len + (float)sqrt( (vec[0]+def->vec[0])*(vec[0]+def->vec[0]) + vec[1]*vec[1] + vec[2]*vec[2]);
560                 
561                 /* def->vec[0]= len limb */
562                 
563                 weight= 1.0f/(0.001f+len);
564                 weight*= weight;
565                 weight*= weight;
566                 weight*= def->fac;
567
568                 len -= def->vec[0];
569
570                 if(def->dist != 0.0) {
571                         if(len >= def->dist) {
572                                 weight= 0.0;
573                         }
574                         else {
575                                 fac= (def->dist - len)/def->dist;
576                                 weight*= fac;
577                         }
578                 }
579                 if(weight > 0.0) {
580                         Mat4MulVecfl(def->postmat, vec);
581                         
582                         VecMulf(vec, weight);
583                         VecAddf(totvec, totvec, vec);
584                 
585                         totw+= weight;
586                 }
587                 def++;
588         }
589         
590         if(totw==0.0) return;
591         
592         co[0]= totvec[0]/totw;
593         co[1]= totvec[1]/totw;
594         co[2]= totvec[2]/totw;
595         
596 }