Initial revision
[blender.git] / source / blender / blenkernel / intern / constraint.c
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version. The Blender
10  * Foundation also sells licenses for use in proprietary software under
11  * the Blender License.  See http://www.blender.org/BL/ for information
12  * about this.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software Foundation,
21  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22  *
23  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
24  * All rights reserved.
25  *
26  * The Original Code is: all of this file.
27  *
28  * Contributor(s): none yet.
29  *
30  * ***** END GPL/BL DUAL LICENSE BLOCK *****
31  */
32
33 #include <stdio.h> 
34 #include <string.h>
35
36 #include "MEM_guardedalloc.h"
37 #include "nla.h"
38
39 #include "BLI_blenlib.h"
40 #include "BLI_arithb.h"
41
42 #include "DNA_armature_types.h"
43 #include "DNA_constraint_types.h"
44 #include "DNA_object_types.h"
45 #include "DNA_action_types.h"
46 #include "DNA_curve_types.h"
47
48 #include "BKE_utildefines.h"
49 #include "BKE_action.h"
50 #include "BKE_armature.h"
51 #include "BKE_blender.h"
52 #include "BKE_constraint.h"
53 #include "BKE_object.h"
54 #include "BKE_ipo.h"
55 #include "BKE_global.h"
56 #include "BKE_library.h"
57
58 #include "blendef.h"
59
60 #ifndef M_PI
61 #define M_PI            3.14159265358979323846
62 #endif
63 /* Local function prototypes */
64 static void constraint_target_to_mat4 (Object *ob, const char *substring, float mat[][4], float size[3], float ctime);
65
66 /* Functions */
67
68 bConstraintChannel *find_constraint_channel (ListBase *list, const char *name){
69         bConstraintChannel *chan;
70
71         for (chan = list->first; chan; chan=chan->next){
72                 if (!strcmp(name, chan->name)){
73                         return chan;
74                 }
75         }
76         return NULL;
77 }
78
79 void do_constraint_channels (ListBase *conbase, ListBase *chanbase, float ctime)
80 {
81         bConstraint *con;
82         bConstraintChannel *chan;
83         IpoCurve *icu;
84
85         for (con=conbase->first; con; con=con->next){
86                 chan = find_constraint_channel(chanbase, con->name);
87                 if (chan && chan->ipo){
88                         calc_ipo(chan->ipo, ctime);
89                         for (icu=chan->ipo->curve.first; icu; icu=icu->next){
90                                 switch (icu->adrcode){
91                                 case CO_ENFORCE:
92                                         con->enforce = icu->curval;
93                                         if (con->enforce<0) con->enforce=0;
94                                         else if (con->enforce>1) con->enforce=1;
95                                         break;
96                                 }
97                         }
98                 }
99         }
100 }
101
102 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
103 {
104         float squat[4], dquat[4], fquat[4];
105         float ssize[3], dsize[3], fsize[4];
106         float sloc[3], dloc[3], floc[3];
107         float mat3[3][3], dstweight;
108         float qmat[3][3], smat[3][3];
109         int i;
110
111
112         dstweight = 1.0F-srcweight;
113
114         Mat3CpyMat4(mat3, dst);
115         Mat3ToQuat(mat3, dquat);
116         Mat3ToSize(mat3, dsize);
117         VECCOPY (dloc, dst[3]);
118
119         Mat3CpyMat4(mat3, src);
120         Mat3ToQuat(mat3, squat);
121         Mat3ToSize(mat3, ssize);
122         VECCOPY (sloc, src[3]);
123         
124         /* Do the actual blend */
125         for (i=0; i<3; i++){
126                 floc[i] = (dloc[i]*dstweight) + (sloc[i]*srcweight);
127                 fsize[i] = 1.0f + ((dsize[i]-1.0f)*dstweight) + ((ssize[i]-1.0f)*srcweight);
128                 fquat[i+1] = (dquat[i+1]*dstweight) + (squat[i+1]*srcweight);
129         }
130         
131         /* Do one more iteration for the quaternions only and normalize the quaternion if needed */
132         fquat[0] = 1.0f + ((dquat[0]-1.0f)*dstweight) + ((squat[0]-1.0f)*srcweight);
133         NormalQuat (fquat);
134
135         QuatToMat3(fquat, qmat);
136         SizeToMat3(fsize, smat);
137
138         Mat3MulMat3(mat3, qmat, smat);
139         Mat4CpyMat3(out, mat3);
140         VECCOPY (out[3], floc);
141 }
142
143 static void constraint_target_to_mat4 (Object *ob, const char *substring, float mat[][4], float size[3], float ctime)
144 {
145
146         /*      Update the location of the target object */
147         where_is_object_time (ob, ctime);       
148
149         /*      Case OBJECT */
150         if (!strlen(substring)){
151                 Mat4CpyMat4 (mat, ob->obmat);
152                 VECCOPY (size, ob->size);
153                 return;
154         }
155
156         /*      Case BONE */
157         else {
158                 bArmature *arm;
159                 Bone    *bone;
160                 float   bmat[4][4];
161                 float   bsize[3]={1, 1, 1};
162
163                 arm = get_armature(ob);
164
165                 /**
166                  *      Locate the bone (if there is one)
167                  *      Ensures that the bone's transformation is fully constrained
168                  *      (Cyclical relationships are disallowed elsewhere)
169                  */
170                 bone = get_named_bone(arm, substring);
171                 if (bone){
172                         where_is_bone_time(ob, bone, ctime);
173                         get_objectspace_bone_matrix(bone, bmat, 1, 1);
174                         VECCOPY(bsize, bone->size);
175                 } 
176                 else
177                         Mat4One (bmat);
178
179                 /**
180                  *      Multiply the objectspace bonematrix by the skeletons's global
181                  *      transform to obtain the worldspace transformation of the target
182                  */
183                 VECCOPY(size, bsize);
184                 Mat4MulMat4 (mat, bmat, ob->obmat);
185         
186                 return; 
187         }
188 }
189
190 void clear_object_constraint_status (Object *ob)
191 {
192         bConstraint *con;
193
194         if (!ob) return;
195
196         /* Clear the object's constraints */
197         for (con = ob->constraints.first; con; con=con->next){
198                 con->flag &= ~CONSTRAINT_DONE;
199         }
200
201         /* Clear the object's subdata constraints */
202         switch (ob->type){
203         case OB_ARMATURE:
204                 {
205                         clear_pose_constraint_status (ob);
206                 }
207                 break;
208         default:
209                 break;
210         }
211 }
212
213 short get_constraint_target (bConstraint *con, short ownertype, void* ownerdata, float mat[][4], float size[3], float ctime)
214 {
215         short valid=0;
216
217         switch (con->type){
218         case CONSTRAINT_TYPE_NULL:
219                 {
220                         Mat4One(mat);
221                 }
222                 break;
223         case CONSTRAINT_TYPE_ACTION:
224                 {
225                         if (ownertype == TARGET_BONE){
226                                 bActionConstraint *data = (bActionConstraint*)con->data;
227                                 bPose *pose=NULL;
228                                 bPoseChannel *pchan=NULL;
229                                 float tempmat[4][4], imat[4][4], ans[4][4], restmat[4][4], irestmat[4][4];
230                                 float tempmat3[3][3];
231                                 float eul[3], size[3];
232                                 float s,t;
233                                 Bone *curBone;
234                                 Bone tbone;
235                                 int i;
236                                 
237                                 curBone = (Bone*)ownerdata;
238                                 
239                                 if (data->tar){
240                                         constraint_target_to_mat4(data->tar, data->subtarget, tempmat, size, ctime);
241                                         valid=1;
242                                 }
243                                 else
244                                         Mat4One (tempmat);
245                                 
246                                 /* If this is a bone, undo parent transforms */
247                                 if (strlen(data->subtarget)){
248                                         Bone* bone;
249
250                                         Mat4Invert(imat, data->tar->obmat);
251                                         bone = get_named_bone(get_armature(data->tar), data->subtarget);
252                                         if (bone){
253                                                 get_objectspace_bone_matrix(bone, restmat, 1, 0);
254                                                 Mat4Invert(irestmat, restmat);
255                                         }
256                                 }
257                                 else{
258                                         Mat4One(imat);
259                                         Mat4One(irestmat);
260                                 }
261
262                                 Mat4MulSerie(ans, imat, tempmat, irestmat, NULL, NULL, NULL, NULL, NULL);
263                                 
264                                 Mat3CpyMat4(tempmat3, ans);
265                                 Mat3ToEul(tempmat3, eul);
266                                 
267                                 eul[0]*=(float)(180.0/M_PI);
268                                 eul[1]*=(float)(180.0/M_PI);
269                                 eul[2]*=(float)(180.0/M_PI);
270
271                                 /* Target is the animation */
272                                 s = (eul[data->type]-data->min)/(data->max-data->min);
273                                 if (s<0)
274                                         s=0;
275                                 if (s>1)
276                                         s=1;
277
278                                 t = ( s * (data->end-data->start)) + data->start;
279                                 
280                                 /* Get the appropriate information from the action */
281                                 pose = MEM_callocN(sizeof(bPose), "pose");
282                                 
283                                 verify_pose_channel(pose, curBone->name);
284                                 get_pose_from_action (&pose, data->act, t);
285
286                                 /* Find the appropriate channel */
287                                 pchan = get_pose_channel(pose, curBone->name);
288                                 if (pchan){
289                                         memset(&tbone, 0x00, sizeof(Bone));
290
291                                         VECCOPY (tbone.loc, pchan->loc);
292                                         VECCOPY (tbone.size, pchan->size);                              
293                                         for (i=0; i<4; i++)
294                                                 tbone.quat[i]=pchan->quat[i];
295                                         
296                                         bone_to_mat4(&tbone, mat);
297
298                                 }
299                                 else{
300                                         Mat4One(mat);
301                                 }
302                                 /* Clean up */
303                                 clear_pose(pose);
304                                 MEM_freeN(pose);
305                         }
306                         
307                 }
308                 break;
309         case CONSTRAINT_TYPE_LOCLIKE:
310                 {
311                         bLocateLikeConstraint *data = (bLocateLikeConstraint*)con->data;
312
313                         if (data->tar){
314                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
315                                 valid=1;
316                         }
317                         else
318                                 Mat4One (mat);
319                 } 
320                 break;
321         case CONSTRAINT_TYPE_ROTLIKE:
322                 {
323                         bRotateLikeConstraint *data;
324                         data = (bRotateLikeConstraint*)con->data;
325
326                         if (data->tar){
327                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
328                                 valid=1;
329                         }
330                         else
331                                 Mat4One (mat);
332                 } 
333                 break;
334         case CONSTRAINT_TYPE_TRACKTO:
335                 {
336                         bTrackToConstraint *data;
337                         data = (bTrackToConstraint*)con->data;
338
339                         if (data->tar){
340                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
341                                 valid=1;
342                         }
343                         else
344                                 Mat4One (mat);
345                 }
346                 break;
347         case CONSTRAINT_TYPE_KINEMATIC:
348                 {
349                         bTrackToConstraint *data;
350                         data = (bTrackToConstraint*)con->data;
351
352                         if (data->tar){
353                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
354                                 valid=1;
355                         }
356                         else
357                                 Mat4One (mat);
358                 } 
359                 break;
360         default:
361                 Mat4One(mat);
362                 break;
363         }
364
365         return valid;
366 }
367
368 void relink_constraints (struct ListBase *list)
369 {
370         bConstraint *con;
371
372         for (con = list->first; con; con=con->next){
373                 switch (con->type){
374                 case CONSTRAINT_TYPE_KINEMATIC:
375                         {
376                                 bKinematicConstraint *data;
377                                 data = con->data;
378
379                                 ID_NEW(data->tar);
380                         }
381                         break;
382                 case CONSTRAINT_TYPE_NULL:
383                         {
384                         }
385                         break;
386                 case CONSTRAINT_TYPE_TRACKTO:
387                         {
388                                 bTrackToConstraint *data;
389                                 data = con->data;
390
391                                 ID_NEW(data->tar);
392                         }
393                         break;
394                 case CONSTRAINT_TYPE_ACTION:
395                         {
396                                 bActionConstraint *data;
397                                 data = con->data;
398
399                                 ID_NEW(data->tar);
400                         }
401                         break;
402                 case CONSTRAINT_TYPE_LOCLIKE:
403                         {
404                                 bLocateLikeConstraint *data;
405                                 data = con->data;
406
407                                 ID_NEW(data->tar);
408                         }
409                         break;
410                 case CONSTRAINT_TYPE_ROTLIKE:
411                         {
412                                 bRotateLikeConstraint *data;
413                                 data = con->data;
414
415                                 ID_NEW(data->tar);
416                         }
417                         break;
418                 }
419         }
420 }
421
422 void *copy_constraint_channels (ListBase *dst, ListBase *src)
423 {
424         bConstraintChannel *dchan, *schan;
425         bConstraintChannel *newact=NULL;
426
427         dst->first=dst->last=NULL;
428         duplicatelist(dst, src);
429         
430         for (dchan=dst->first, schan=src->first; dchan; dchan=dchan->next, schan=schan->next){
431                 dchan->ipo = copy_ipo(schan->ipo);
432         }
433
434         return newact;
435 }
436
437 bConstraintChannel *clone_constraint_channels (ListBase *dst, ListBase *src, bConstraintChannel *oldact)
438 {
439         bConstraintChannel *dchan, *schan;
440         bConstraintChannel *newact=NULL;
441
442         dst->first=dst->last=NULL;
443         duplicatelist(dst, src);
444         
445         for (dchan=dst->first, schan=src->first; dchan; dchan=dchan->next, schan=schan->next){
446                 id_us_plus((ID *)dchan->ipo);
447                 if (schan==oldact)
448                         newact=dchan;
449         }
450
451         return newact;
452 }
453
454 void copy_constraints (ListBase *dst, ListBase *src)
455 {
456         bConstraint *con;
457
458         dst->first=dst->last=NULL;
459
460         duplicatelist (dst, src);
461
462         /* Update specific data */
463         if (!dst->first)
464                 return;
465
466         for (con = dst->first; con; con=con->next){
467                 switch (con->type){
468                 case CONSTRAINT_TYPE_ACTION:
469                         {
470                                 bActionConstraint *data;
471
472                                 con->data = MEM_dupallocN (con->data);
473                                 data = (bActionConstraint*) con->data;
474                         }
475                         break;
476                 case CONSTRAINT_TYPE_LOCLIKE:
477                         {
478                                 bLocateLikeConstraint *data;
479
480                                 con->data = MEM_dupallocN (con->data);
481                                 data = (bLocateLikeConstraint*) con->data;
482                         }
483                         break;
484                 case CONSTRAINT_TYPE_ROTLIKE:
485                         {
486                                 bRotateLikeConstraint *data;
487                                 
488                                 con->data = MEM_dupallocN (con->data);
489                                 data = (bRotateLikeConstraint*) con->data;
490                         }
491                         break;
492                 case CONSTRAINT_TYPE_NULL:
493                         {
494                                 con->data = NULL;
495                         }
496                         break;
497                 case CONSTRAINT_TYPE_TRACKTO:
498                         {
499                                 bTrackToConstraint *data;
500                                 
501                                 con->data = MEM_dupallocN (con->data);
502                                 data = (bTrackToConstraint*) con->data;
503                         }
504                         break;
505                 case CONSTRAINT_TYPE_KINEMATIC:
506                         {
507                                 bKinematicConstraint *data;
508                                 
509                                 con->data = MEM_dupallocN (con->data);
510                                 data = (bKinematicConstraint*) con->data;
511                         }
512                         break;
513                 default:
514                         con->data = MEM_dupallocN (con->data);
515                         break;
516                 }
517         }
518 }
519
520 void evaluate_constraint (bConstraint *constraint, Object *ob, short ownertype, void *ownerdata, float targetmat[][4])
521 /* ob is likely to be a workob */
522 {
523         float   M_oldmat[4][4];
524         float   M_identity[4][4];
525         float   enforce = 1.0;
526         
527         if (!constraint || !ob)
528                 return;
529         
530         Mat4One (M_identity);
531         
532         /* We've already been calculated */
533         if (constraint->flag & CONSTRAINT_DONE){
534                 return;
535         }
536         
537         switch (constraint->type){
538         case CONSTRAINT_TYPE_ACTION:
539                 {
540                         float temp[4][4];
541                         bActionConstraint *data;
542                         
543                         data = constraint->data;
544                         Mat4CpyMat4 (temp, ob->obmat);
545
546                         Mat4MulMat4(ob->obmat, targetmat, temp);
547                 }
548                 break;
549         case CONSTRAINT_TYPE_LOCLIKE:
550                 {
551                         bLocateLikeConstraint *data;
552
553                         data = constraint->data;
554                         
555                         if (data->flag & LOCLIKE_X)
556                                 ob->obmat[3][0] = targetmat[3][0];
557                         if (data->flag & LOCLIKE_Y)
558                                 ob->obmat[3][1] = targetmat[3][1];
559                         if (data->flag & LOCLIKE_Z)
560                                 ob->obmat[3][2] = targetmat[3][2];
561                 }
562                 break;
563         case CONSTRAINT_TYPE_ROTLIKE:
564                 {
565                 float   tmat[4][4];
566                 float   size[3];
567
568                 Mat4ToSize(ob->obmat, size);
569                 
570                 Mat4CpyMat4 (tmat, targetmat);
571                 Mat4Ortho(tmat);
572
573                 ob->obmat[0][0] = tmat[0][0]*size[0];
574                 ob->obmat[0][1] = tmat[0][1]*size[1];
575                 ob->obmat[0][2] = tmat[0][2]*size[2];
576
577                 ob->obmat[1][0] = tmat[1][0]*size[0];
578                 ob->obmat[1][1] = tmat[1][1]*size[1];
579                 ob->obmat[1][2] = tmat[1][2]*size[2];
580
581                 ob->obmat[2][0] = tmat[2][0]*size[0];
582                 ob->obmat[2][1] = tmat[2][1]*size[1];
583                 ob->obmat[2][2] = tmat[2][2]*size[2];
584                 }
585                 break;
586         case CONSTRAINT_TYPE_NULL:
587                 {
588                 }
589                 break;
590         case CONSTRAINT_TYPE_TRACKTO:
591                 {
592                         bTrackToConstraint *data;
593                         float   size[3];
594
595                         data=(bTrackToConstraint*)constraint->data;                     
596                         
597                         if (data->tar){
598                                         
599                                 Mat4ToSize (ob->obmat, size);
600         
601                                 Mat4CpyMat4 (M_oldmat, ob->obmat);
602
603                                 // Clear the object's rotation  
604                                 ob->obmat[0][0]=ob->size[0];
605                                 ob->obmat[0][1]=0;
606                                 ob->obmat[0][2]=0;
607                                 ob->obmat[1][0]=0;
608                                 ob->obmat[1][1]=ob->size[1];
609                                 ob->obmat[1][2]=0;
610                                 ob->obmat[2][0]=0;
611                                 ob->obmat[2][1]=0;
612                                 ob->obmat[2][2]=ob->size[2];
613         
614                                 solve_tracking(ob, targetmat);
615                         }
616                 }
617                 break;
618         case CONSTRAINT_TYPE_KINEMATIC:
619                 {
620                         bKinematicConstraint *data;
621                         float   imat[4][4];
622                         float   temp[4][4];
623                         float totmat[4][4];
624
625                         data=(bKinematicConstraint*)constraint->data;
626
627                         if (data->tar && ownertype==TARGET_BONE && ownerdata){
628                                 Bone *curBone = (Bone*)ownerdata;
629                                 PoseChain *chain;
630                                 Object *armob;
631                                 
632                                 /* Retrieve the owner armature object from the workob */
633                                 armob = ob->parent;     
634                                 
635                                 /*      Make an IK chain  */                             
636                                 chain = ik_chain_to_posechain(armob, curBone);
637                                 if (!chain)
638                                         return;
639                                 chain->iterations = data->iterations;
640                                 chain->tolerance = data->tolerance;
641                                 
642                                 
643                                 {
644                                         float parmat[4][4];
645                                         
646                                         /* Take the obmat to objectspace */
647                                         Mat4CpyMat4 (temp, curBone->obmat);
648                                         Mat4One (curBone->obmat);
649                                         get_objectspace_bone_matrix(curBone, parmat, 1, 1);
650                                         Mat4CpyMat4 (curBone->obmat, temp);
651                                         Mat4MulMat4 (totmat, parmat, ob->parent->obmat);
652                                         
653                                         Mat4Invert (imat, totmat);
654                                         
655                                         Mat4CpyMat4 (temp, ob->obmat);
656                                         Mat4MulMat4 (ob->obmat, temp, imat);
657                                 }
658                                 
659                                 
660                                 /* Solve it */
661                                 if (chain->solver){
662                                         VECCOPY (chain->goal, targetmat[3]);                                    
663                                         solve_posechain(chain);
664                                 }
665                                 
666                                 free_posechain(chain);
667                                 
668                                 {
669                                         float parmat[4][4];
670                                         
671                                         /* Take the obmat to worldspace */
672                                         Mat4CpyMat4 (temp, curBone->obmat);
673                                         Mat4One (curBone->obmat);
674                                         get_objectspace_bone_matrix(curBone, parmat, 1, 1);
675                                         Mat4CpyMat4 (curBone->obmat, temp);
676                                         Mat4MulMat4 (totmat, parmat, ob->parent->obmat);
677                                         
678                                         Mat4CpyMat4 (temp, ob->obmat);
679                                         Mat4MulMat4 (ob->obmat, temp, totmat);
680                                         
681                                 }
682                         }
683                 }
684                 break;
685         case CONSTRAINT_TYPE_FOLLOWPATH:
686                 break;
687         default:
688                 printf ("Error: Unknown constraint type\n");
689                 break;
690         }
691
692 }
693
694 void free_constraint_data (bConstraint *con)
695 {
696         if (con->data){
697                 switch (con->type){
698                 default:
699                         break;
700                 };
701                 
702                 MEM_freeN (con->data);
703         }
704 }
705
706 void free_constraints (ListBase *conlist)
707 {
708         bConstraint *con;
709
710         /* Do any specific freeing */
711         for (con=conlist->first; con; con=con->next)
712         {
713                 free_constraint_data (con);
714         };
715
716         /* Free the whole list */
717         BLI_freelistN(conlist);
718 }
719
720 void free_constraint_channels (ListBase *chanbase)
721 {
722         bConstraintChannel *chan;
723
724         for (chan=chanbase->first; chan; chan=chan->next)
725         {
726                 if (chan->ipo){
727                         chan->ipo->id.us--;
728                 }
729         }
730
731         BLI_freelistN(chanbase);
732 }