== Constraints System ==
[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): 2007, Joshua Leung, major recode
29  *
30  * ***** END GPL/BL DUAL LICENSE BLOCK *****
31  */
32
33 #include <stdio.h> 
34 #include <string.h>
35 #include <math.h>
36
37 #include "MEM_guardedalloc.h"
38 #include "nla.h"
39
40 #include "BLI_blenlib.h"
41 #include "BLI_arithb.h"
42
43 #include "DNA_armature_types.h"
44 #include "DNA_constraint_types.h"
45 #include "DNA_object_types.h"
46 #include "DNA_action_types.h"
47 #include "DNA_curve_types.h"
48 #include "DNA_scene_types.h"
49
50 #include "BKE_utildefines.h"
51 #include "BKE_action.h"
52 #include "BKE_anim.h" // for the curve calculation part
53 #include "BKE_armature.h"
54 #include "BKE_blender.h"
55 #include "BKE_constraint.h"
56 #include "BKE_displist.h"
57 #include "BKE_object.h"
58 #include "BKE_ipo.h"
59 #include "BKE_global.h"
60 #include "BKE_library.h"
61 #include "BKE_idprop.h"
62
63 #include "BPY_extern.h"
64
65 #include "blendef.h"
66
67 #ifdef HAVE_CONFIG_H
68 #include <config.h>
69 #endif
70
71 #ifndef M_PI
72 #define M_PI            3.14159265358979323846
73 #endif
74
75
76 /* ********************* Data level ****************** */
77
78 void free_constraint_data (bConstraint *con)
79 {
80         if (con->data) {
81                 /* any constraint-type specific stuff here */
82                 switch (con->type) {
83                         case CONSTRAINT_TYPE_PYTHON:
84                         {
85                                 bPythonConstraint *data= con->data;
86                                 IDP_FreeProperty(data->prop);
87                                 MEM_freeN(data->prop);
88                         }
89                                 break;
90                 }
91                 
92                 MEM_freeN(con->data);
93         }
94 }
95
96 void free_constraints (ListBase *conlist)
97 {
98         bConstraint *con;
99         
100         /* Do any specific freeing */
101         for (con=conlist->first; con; con=con->next) {
102                 free_constraint_data(con);
103         }
104         
105         /* Free the whole list */
106         BLI_freelistN(conlist);
107 }
108
109 void free_constraint_channels (ListBase *chanbase)
110 {
111         bConstraintChannel *chan;
112         
113         for (chan=chanbase->first; chan; chan=chan->next) {
114                 if (chan->ipo) {
115                         chan->ipo->id.us--;
116                 }
117         }
118         
119         BLI_freelistN(chanbase);
120 }
121
122 void relink_constraints (struct ListBase *list)
123 {
124         bConstraint *con;
125         
126         for (con = list->first; con; con=con->next) {
127                 /* check if constraint has a target that needs relinking */
128                 if (constraint_has_target(con)) {
129                         Object *tar;
130                         char *subtarget;
131                         
132                         tar = get_constraint_target(con, &subtarget);
133                         ID_NEW(tar);
134                 }
135         }
136 }
137
138 void copy_constraint_channels (ListBase *dst, ListBase *src)
139 {
140         bConstraintChannel *dchan, *schan;
141         
142         dst->first=dst->last=NULL;
143         duplicatelist(dst, src);
144         
145         for (dchan=dst->first, schan=src->first; dchan; dchan=dchan->next, schan=schan->next) {
146                 dchan->ipo = copy_ipo(schan->ipo);
147         }
148 }
149
150 void clone_constraint_channels (ListBase *dst, ListBase *src)
151 {
152         bConstraintChannel *dchan, *schan;
153         
154         dst->first=dst->last=NULL;
155         duplicatelist(dst, src);
156         
157         for (dchan=dst->first, schan=src->first; dchan; dchan=dchan->next, schan=schan->next) {
158                 id_us_plus((ID *)dchan->ipo);
159         }
160 }
161
162 void copy_constraints (ListBase *dst, ListBase *src)
163 {
164         bConstraint *con, *srccon;
165         
166         dst->first= dst->last= NULL;
167         duplicatelist (dst, src);
168         
169         for (con = dst->first, srccon=src->first; con; srccon=srccon->next, con=con->next) {
170                 con->data = MEM_dupallocN (con->data);
171                 
172                 /* only do specific constraints if required */
173                 if (con->type == CONSTRAINT_TYPE_PYTHON) {
174                         bPythonConstraint *pycon = (bPythonConstraint *)con->data;
175                         bPythonConstraint *opycon = (bPythonConstraint *)srccon->data;
176                         
177                         pycon->prop = IDP_CopyProperty(opycon->prop);
178                 }
179         }
180 }
181
182 /* **************** Editor Functions **************** */
183
184 char constraint_has_target (bConstraint *con) 
185 {
186         switch (con->type) {
187         case CONSTRAINT_TYPE_PYTHON:
188                 {
189                         bPythonConstraint *data = con->data;
190                         if (data->tar) return 1;
191                 }
192                 break;
193         case CONSTRAINT_TYPE_TRACKTO:
194                 {
195                         bTrackToConstraint *data = con->data;
196                         if (data->tar) return 1;
197                 }
198                 break;
199         case CONSTRAINT_TYPE_KINEMATIC:
200                 {
201                         bKinematicConstraint *data = con->data;
202                         if (data->tar) return 1;
203                 }
204                 break;
205         case CONSTRAINT_TYPE_FOLLOWPATH:
206                 {
207                         bFollowPathConstraint *data = con->data;
208                         if (data->tar) return 1;
209                 }
210                 break;
211         case CONSTRAINT_TYPE_ROTLIKE:
212                 {
213                         bRotateLikeConstraint *data = con->data;
214                         if (data->tar) return 1;
215                 }
216                 break;
217         case CONSTRAINT_TYPE_LOCLIKE:
218                 {
219                         bLocateLikeConstraint *data = con->data;
220                         if (data->tar) return 1;
221                 }
222                 break;
223         case CONSTRAINT_TYPE_SIZELIKE:
224                 {
225                         bSizeLikeConstraint *data = con->data;
226                         if (data->tar) return 1;
227                 }
228                 break;
229         case CONSTRAINT_TYPE_MINMAX:
230                 {
231                         bMinMaxConstraint *data = con->data;
232                         if (data->tar) return 1;
233                 }
234                 break;
235         case CONSTRAINT_TYPE_ACTION:
236                 {
237                         bActionConstraint *data = con->data;
238                         if (data->tar) return 1;
239                 }
240                 break;
241         case CONSTRAINT_TYPE_LOCKTRACK:
242                 {
243                         bLockTrackConstraint *data = con->data;
244                         if (data->tar) return 1;
245                 }
246         case CONSTRAINT_TYPE_STRETCHTO:
247                 {
248                         bStretchToConstraint *data = con->data;
249                         if (data->tar) return 1;
250                 }
251                 break;
252         case CONSTRAINT_TYPE_RIGIDBODYJOINT:
253                 {
254                         bRigidBodyJointConstraint *data = con->data;
255                         if (data->tar) return 1;
256                 }
257                 break;
258         case CONSTRAINT_TYPE_CLAMPTO:
259                 {
260                         bClampToConstraint *data = con->data;
261                         if (data->tar) return 1;
262                 }
263                         break;
264         case CONSTRAINT_TYPE_CHILDOF:
265                 {
266                         bChildOfConstraint *data = con->data;
267                         if (data->tar) return 1;
268                 }
269                 break;
270         }
271         
272         /* Unknown types or CONSTRAINT_TYPE_NULL or no target */
273         return 0;
274 }
275
276 Object *get_constraint_target(bConstraint *con, char **subtarget)
277 {
278         /* If the target for this constraint is target, return a pointer 
279          * to the name for this constraints subtarget ... NULL otherwise
280          */
281         switch (con->type) {
282         case CONSTRAINT_TYPE_PYTHON:
283                 {
284                         bPythonConstraint *data=con->data;
285                         *subtarget = data->subtarget;
286                         return data->tar;
287                 }
288                 break;
289         case CONSTRAINT_TYPE_ACTION:
290                 {
291                         bActionConstraint *data = con->data;
292                         *subtarget= data->subtarget;
293                         return data->tar;
294                 }
295                 break;
296         case CONSTRAINT_TYPE_LOCLIKE:
297                 {
298                         bLocateLikeConstraint *data = con->data;
299                         *subtarget= data->subtarget;
300                         return data->tar;
301                 }
302                 break;
303         case CONSTRAINT_TYPE_ROTLIKE:
304                 {
305                         bRotateLikeConstraint *data = con->data;
306                         *subtarget= data->subtarget;
307                         return data->tar;
308                 }
309                 break;
310         case CONSTRAINT_TYPE_SIZELIKE:
311                 {
312                         bSizeLikeConstraint *data = con->data;
313                         *subtarget= data->subtarget;
314                         return data->tar;
315                 }
316                 break;
317         case CONSTRAINT_TYPE_KINEMATIC:
318                 {
319                         bKinematicConstraint *data = con->data;
320                         *subtarget= data->subtarget;
321                         return data->tar;
322                 }
323                 break;
324         case CONSTRAINT_TYPE_TRACKTO:
325                 {
326                         bTrackToConstraint *data = con->data;
327                         *subtarget= data->subtarget;
328                         return data->tar;
329                 }
330                 break;
331         case CONSTRAINT_TYPE_MINMAX:
332                 {
333                         bMinMaxConstraint *data = con->data;
334                         *subtarget= data->subtarget;
335                         return data->tar;
336                 }
337                 break;
338         case CONSTRAINT_TYPE_LOCKTRACK:
339                 {
340                         bLockTrackConstraint *data = con->data;
341                         *subtarget= data->subtarget;
342                         return data->tar;
343                 }
344                 break;
345         case CONSTRAINT_TYPE_FOLLOWPATH: 
346                 {
347                         bFollowPathConstraint *data = con->data;
348                         *subtarget= NULL;
349                         return data->tar;
350                 }
351                 break;
352         case CONSTRAINT_TYPE_STRETCHTO:
353                 {
354                         bStretchToConstraint *data = con->data;
355                         *subtarget= data->subtarget;
356                         return data->tar;
357                 }
358                 break;
359         case CONSTRAINT_TYPE_RIGIDBODYJOINT: 
360                 {
361                         bRigidBodyJointConstraint *data = con->data;
362                         *subtarget= NULL;
363                         return data->tar;
364                 }
365                 break;
366         case CONSTRAINT_TYPE_CLAMPTO:
367                 {
368                         bClampToConstraint *data = con->data;
369                         *subtarget= NULL;
370                         return data->tar;
371                 }
372                 break;
373         case CONSTRAINT_TYPE_CHILDOF:   
374                 {
375                         bChildOfConstraint *data = con->data;
376                         *subtarget= data->subtarget;
377                         return data->tar;
378                 }
379                 break;
380         default:
381                 *subtarget= NULL;
382                 break;
383         }
384         
385         return NULL;  
386 }
387
388 void set_constraint_target(bConstraint *con, Object *ob, char *subtarget)
389 {
390         /* Set the target for this constraint */
391         switch (con->type) {
392                 case CONSTRAINT_TYPE_PYTHON:
393                 {
394                         bPythonConstraint *data = con->data;
395                         data->tar= ob;
396                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
397                 }
398                         break;          
399                 case CONSTRAINT_TYPE_ACTION:
400                 {
401                         bActionConstraint *data = con->data;
402                         data->tar= ob;
403                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
404                 }
405                         break;
406                 case CONSTRAINT_TYPE_LOCLIKE:
407                 {
408                         bLocateLikeConstraint *data = con->data;
409                         data->tar= ob;
410                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
411                 }
412                         break;
413                 case CONSTRAINT_TYPE_ROTLIKE:
414                 {
415                         bRotateLikeConstraint *data = con->data;
416                         data->tar= ob;
417                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
418                 }
419                         break;
420                 case CONSTRAINT_TYPE_SIZELIKE:
421                 {
422                         bSizeLikeConstraint *data = con->data;
423                         data->tar= ob;
424                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
425                 }
426                         break;
427                 case CONSTRAINT_TYPE_KINEMATIC:
428                 {
429                         bKinematicConstraint *data = con->data;
430                         data->tar= ob;
431                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
432                 }
433                         break;
434                 case CONSTRAINT_TYPE_TRACKTO:
435                 {
436                         bTrackToConstraint *data = con->data;
437                         data->tar= ob;
438                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
439                 }
440                         break;
441                 case CONSTRAINT_TYPE_LOCKTRACK:
442                 {
443                         bLockTrackConstraint *data = con->data;
444                         data->tar= ob;
445                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
446                 }
447                         break;
448                 case CONSTRAINT_TYPE_FOLLOWPATH: 
449                 {
450                         bFollowPathConstraint *data = con->data;
451                         data->tar= ob;
452                 }
453                         break;
454                 case CONSTRAINT_TYPE_STRETCHTO:
455                 {
456                         bStretchToConstraint *data = con->data;
457                         data->tar= ob;
458                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
459                 }
460                         break;
461                 case CONSTRAINT_TYPE_RIGIDBODYJOINT: 
462                 {
463                         bRigidBodyJointConstraint *data = con->data;
464                         data->tar= ob;
465                 }
466                         break;
467                 case CONSTRAINT_TYPE_MINMAX:
468                 {
469                         bMinMaxConstraint *data = (bMinMaxConstraint*)con->data;
470                         data->tar= ob;
471                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
472                 }
473                         break;
474                 case CONSTRAINT_TYPE_CLAMPTO: 
475                 {
476                         bClampToConstraint *data = con->data;
477                         data->tar= ob;
478                 }
479                         break;
480                 case CONSTRAINT_TYPE_CHILDOF:
481                 {
482                         bChildOfConstraint *data = con->data;
483                         data->tar= ob;
484                         if (subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
485                 }
486                         break;
487         }
488 }
489
490 void unique_constraint_name (bConstraint *con, ListBase *list)
491 {
492         bConstraint *curcon;
493         char tempname[64];
494         int     number = 1, exists = 0;
495         char *dot;
496         
497         /* See if we are given an empty string */
498         if (con->name[0] == '\0') {
499                 /* give it default name first */
500                 strcpy(con->name, "Const");
501         }
502         
503         /* See if we even need to do this */
504         for (curcon = list->first; curcon; curcon=curcon->next) {
505                 if (curcon != con) {
506                         if (!strcmp(curcon->name, con->name)) {
507                                 exists = 1;
508                                 break;
509                         }
510                 }
511         }
512         
513         if (!exists)
514                 return;
515
516         /*      Strip off the suffix */
517         dot=strchr(con->name, '.');
518         if (dot)
519                 *dot=0;
520         
521         for (number = 1; number <= 999; number++) {
522                 sprintf(tempname, "%s.%03d", con->name, number);
523                 
524                 exists = 0;
525                 for (curcon=list->first; curcon; curcon=curcon->next) {
526                         if (con!=curcon) {
527                                 if (!strcmp(curcon->name, tempname)) {
528                                         exists = 1;
529                                         break;
530                                 }
531                         }
532                 }
533                 if (!exists) {
534                         strcpy(con->name, tempname);
535                         return;
536                 }
537         }
538 }
539
540 void *new_constraint_data (short type)
541 {
542         void *result;
543         
544         switch (type) {
545         case CONSTRAINT_TYPE_PYTHON:
546                 {
547                         bPythonConstraint *data;
548                         data = MEM_callocN(sizeof(bPythonConstraint), "pythonConstraint");
549                         
550                         /* everything should be set correctly by calloc, except for the prop->type constant.*/
551                         data->prop = MEM_callocN(sizeof(IDProperty), "PyConstraintProps");
552                         data->prop->type = IDP_GROUP;
553                         
554                         result = data;
555                 }
556                 break;          
557         case CONSTRAINT_TYPE_KINEMATIC:
558                 {
559                         bKinematicConstraint *data;
560                         data = MEM_callocN(sizeof(bKinematicConstraint), "kinematicConstraint");
561
562                         data->weight= (float)1.0;
563                         data->orientweight= (float)1.0;
564                         data->iterations = 500;
565                         data->flag= CONSTRAINT_IK_TIP|CONSTRAINT_IK_STRETCH|CONSTRAINT_IK_POS;
566                         
567                         result = data;
568                 }
569                 break;
570         case CONSTRAINT_TYPE_TRACKTO:
571                 {
572                         bTrackToConstraint *data;
573                         data = MEM_callocN(sizeof(bTrackToConstraint), "tracktoConstraint");
574                         
575                         data->reserved1 = TRACK_Y;
576                         data->reserved2 = UP_Z;
577                         
578                         result = data;
579                 }
580                 break;
581         case CONSTRAINT_TYPE_MINMAX:
582                 {
583                         bMinMaxConstraint *data;
584                         data = MEM_callocN(sizeof(bMinMaxConstraint), "minmaxConstraint");
585                         
586                         data->minmaxflag = TRACK_Z;
587                         data->offset = 0.0f;
588                         data->cache[0] = data->cache[1] = data->cache[2] = 0.0f;
589                         data->flag = 0;
590                         
591                         result = data;
592                 }
593                 break;
594         case CONSTRAINT_TYPE_LOCLIKE:
595                 {
596                         bLocateLikeConstraint *data;
597                         data = MEM_callocN(sizeof(bLocateLikeConstraint), "LocLikeConstraint");
598                         data->flag = LOCLIKE_X|LOCLIKE_Y|LOCLIKE_Z;
599                         result = data;
600                 }
601                 break;
602         case CONSTRAINT_TYPE_ROTLIKE:
603                 {
604                         bRotateLikeConstraint *data;
605                         data = MEM_callocN(sizeof(bRotateLikeConstraint), "RotLikeConstraint");
606                         data->flag = ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z;
607                         result = data;
608                 }
609                 break;
610         case CONSTRAINT_TYPE_SIZELIKE:
611                 {
612                         bSizeLikeConstraint *data;
613                         data = MEM_callocN(sizeof(bLocateLikeConstraint), "SizeLikeConstraint");
614                         data->flag = SIZELIKE_X|SIZELIKE_Y|SIZELIKE_Z;
615                         result = data;
616                 }
617                 break;
618         case CONSTRAINT_TYPE_ACTION:
619                 {
620                         bActionConstraint *data;
621                         data = MEM_callocN(sizeof(bActionConstraint), "ActionConstraint");
622                         
623                         /* set type to 20 (Loc X), as 0 is Rot X for backwards compatability */
624                         data->type = 20;
625                         
626                         result = data;
627                 }
628                 break;
629         case CONSTRAINT_TYPE_LOCKTRACK:
630                 {
631                         bLockTrackConstraint *data;
632                         data = MEM_callocN(sizeof(bLockTrackConstraint), "locktrackConstraint");
633                         
634                         data->trackflag = TRACK_Y;
635                         data->lockflag = LOCK_Z;
636                         
637                         result = data;
638                 }
639                 break;
640         case CONSTRAINT_TYPE_FOLLOWPATH:
641                 {
642                         bFollowPathConstraint *data;
643                         data = MEM_callocN(sizeof(bFollowPathConstraint), "followpathConstraint");
644
645                         data->trackflag = TRACK_Y;
646                         data->upflag = UP_Z;
647                         data->offset = 0;
648                         data->followflag = 0;
649
650                         result = data;
651                 }
652                 break;
653         case CONSTRAINT_TYPE_STRETCHTO:
654                 {
655                         bStretchToConstraint *data;
656                         data = MEM_callocN(sizeof(bStretchToConstraint), "StretchToConstraint");
657
658                         data->volmode = 0;
659                         data->plane = 0;
660                         data->orglength = 0.0; 
661                         data->bulge = 1.0;
662                         result = data;
663                 }
664                 break; 
665         case CONSTRAINT_TYPE_LOCLIMIT:
666                 {
667                         bLocLimitConstraint *data;
668                         data = MEM_callocN(sizeof(bLocLimitConstraint), "LocLimitConstraint");
669                         result = data;
670                 }
671                 break;
672         case CONSTRAINT_TYPE_ROTLIMIT:
673                 {
674                         bRotLimitConstraint *data;
675                         data = MEM_callocN(sizeof(bRotLimitConstraint), "RotLimitConstraint");
676                         result = data;
677                 }
678                 break;
679         case CONSTRAINT_TYPE_SIZELIMIT:
680                 {
681                         bSizeLimitConstraint *data;
682                         data = MEM_callocN(sizeof(bSizeLimitConstraint), "SizeLimitConstraint");
683                         result = data;
684                 }
685                 break;
686     case CONSTRAINT_TYPE_RIGIDBODYJOINT:
687                 {
688                         bRigidBodyJointConstraint *data;
689                         data = MEM_callocN(sizeof(bRigidBodyJointConstraint), "RigidBodyToConstraint");
690                         
691                         // removed code which set target of this constraint  
692             data->type=1;
693                         
694                         result = data;
695                 }
696                 break;
697         case CONSTRAINT_TYPE_CLAMPTO:
698                 {
699                         bClampToConstraint *data;
700                         data = MEM_callocN(sizeof(bClampToConstraint), "ClampToConstraint");
701                         result = data;
702                 }
703                 break;
704         case CONSTRAINT_TYPE_CHILDOF:
705                 {
706                         bChildOfConstraint *data;
707                         data = MEM_callocN(sizeof(bChildOfConstraint), "ChildOfConstraint");
708                         
709                         data->flag = (CHILDOF_LOCX | CHILDOF_LOCY | CHILDOF_LOCZ |
710                                                         CHILDOF_ROTX |CHILDOF_ROTY | CHILDOF_ROTZ |
711                                                         CHILDOF_SIZEX | CHILDOF_SIZEY | CHILDOF_SIZEZ);
712                         Mat4One(data->invmat);
713                         
714                         result = data;
715                 }
716                 break;
717   
718         default:
719                 result = NULL;
720                 break;
721         }
722
723         return result;
724 }
725
726 bConstraintChannel *get_constraint_channel (ListBase *list, const char *name)
727 {
728         bConstraintChannel *chan;
729
730         for (chan = list->first; chan; chan=chan->next) {
731                 if (!strcmp(name, chan->name)) {
732                         return chan;
733                 }
734         }
735         return NULL;
736 }
737
738 /* finds or creates new constraint channel */
739 bConstraintChannel *verify_constraint_channel (ListBase *list, const char *name)
740 {
741         bConstraintChannel *chan;
742         
743         chan= get_constraint_channel (list, name);
744         if(chan==NULL) {
745                 chan= MEM_callocN(sizeof(bConstraintChannel), "new constraint chan");
746                 BLI_addtail(list, chan);
747                 strcpy(chan->name, name);
748         }
749         
750         return chan;
751 }
752
753
754 /* ***************** Evaluating ********************* */
755
756 /* package an object/bone for use in constraint evaluation */
757 /* This function MEM_calloc's a bConstraintOb struct, that will need to be freed after evaluation */
758 bConstraintOb *constraints_make_evalob (Object *ob, void *subdata, short datatype)
759 {
760         bConstraintOb *cob;
761         
762         /* create regardless of whether we have any data! */
763         cob= MEM_callocN(sizeof(bConstraintOb), "bConstraintOb");
764         
765         /* based on type of available data */
766         switch (datatype) {
767                 case TARGET_OBJECT:
768                 {
769                         /* disregard subdata... calloc should set other values right */
770                         if (ob) {
771                                 cob->ob = ob;
772                                 cob->type = datatype;
773                                 Mat4CpyMat4(cob->matrix, ob->obmat);
774                                 Mat4CpyMat4(cob->startmat, ob->obmat);
775                         }
776                         else 
777                                 Mat4One(cob->matrix);
778                 }
779                         break;
780                 case TARGET_BONE:
781                 {
782                         /* only set if we have valid bone, otherwise default */
783                         if (ob && subdata) {
784                                 cob->ob = ob;
785                                 cob->pchan = (bPoseChannel *)subdata;
786                                 cob->type = datatype;
787                                 
788                                 /* matrix in world-space */
789                                 Mat4MulMat4 (cob->matrix, cob->pchan->pose_mat, ob->obmat);
790                                 Mat4CpyMat4(cob->startmat, cob->matrix);
791                         }
792                         else
793                                 Mat4One(cob->matrix);
794                                 Mat4CpyMat4(cob->startmat, cob->matrix);
795                 }
796                         break;
797                         
798                 default: // other types not yet handled
799                         Mat4One(cob->matrix);
800                         break;
801         }
802         
803         return cob;
804 }
805
806 /* cleanup after constraint evaluation */
807 void constraints_clear_evalob (bConstraintOb *cob)
808 {
809         float delta[4][4], imat[4][4];
810         
811         /* prevent crashes */
812         if (cob == NULL) 
813                 return;
814         
815         /* calculate delta of constraints evaluation */
816         Mat4Invert(imat, cob->startmat);
817         Mat4MulMat4(delta, cob->matrix, imat);
818         
819         /* copy matrices back to source */
820         switch (cob->type) {
821                 case TARGET_OBJECT:
822                 {
823                         /* copy new ob-matrix back to owner */
824                         Mat4CpyMat4(cob->ob->obmat, cob->matrix);
825                         
826                         /* copy inverse of delta back to owner */
827                         Mat4Invert(cob->ob->constinv, delta);
828                 }
829                         break;
830                 case TARGET_BONE:
831                 {
832                         /* copy new pose-matrix back to owner */
833                         Mat4MulMat4(cob->pchan->pose_mat, cob->matrix, cob->ob->imat);
834                         
835                         /* copy inverse of delta back to owner */
836                         Mat4Invert(cob->pchan->constinv, delta);
837                 }
838                         break;
839         }
840         
841         /* free tempolary struct */
842         MEM_freeN(cob);
843 }
844
845 /* -------------------------------- Constraint Channels ---------------------------- */
846
847 /* does IPO's of constraint channels only */
848 void do_constraint_channels (ListBase *conbase, ListBase *chanbase, float ctime)
849 {
850         bConstraint *con;
851         bConstraintChannel *chan;
852         IpoCurve *icu= NULL;
853         
854         /* for each Constraint, calculate its Influence from the corresponding ConstraintChannel */
855         for (con=conbase->first; con; con=con->next) {
856                 chan = get_constraint_channel(chanbase, con->name);
857                 if (chan && chan->ipo) {
858                         calc_ipo(chan->ipo, ctime);
859                         for (icu=chan->ipo->curve.first; icu; icu=icu->next) {
860                                 switch (icu->adrcode) {
861                                         case CO_ENFORCE:
862                                         {
863                                                 con->enforce = icu->curval;
864                                                 if (con->enforce<0.0f) con->enforce= 0.0f;
865                                                 else if (con->enforce>1.0f) con->enforce= 1.0f;
866                                         }
867                                                 break;
868                                 }
869                         }
870                 }
871         }
872 }
873
874 /* ------------------------------- Space-Conversion API ---------------------------- */
875
876 /* This function is responsible for the correct transformations/conversions 
877  * of a matrix from one space to another for constraint evaluation.
878  * For now, this is only implemented for Objects and PoseChannels.
879  */
880 static void constraint_mat_convertspace (Object *ob, bPoseChannel *pchan, float mat[][4], short from, short to)
881 {
882         float tempmat[4][4];
883         float diff_mat[4][4];
884         float imat[4][4];
885         
886         /* prevent crashes in these unlikely events  */
887         if (ob==NULL || mat==NULL) return;
888         /* optimise trick - check if need to do anything */
889         if (from == to) return;
890         
891         /* are we dealing with pose-channels or objects */
892         if (pchan) {
893                 /* pose channels */
894                 switch (from) {
895                         case CONSTRAINT_SPACE_WORLD: /* ---------- FROM WORLDSPACE ---------- */
896                         {
897                                 /* world to pose */
898                                 if (to==CONSTRAINT_SPACE_POSE || to==CONSTRAINT_SPACE_LOCAL || to==CONSTRAINT_SPACE_PARLOCAL) {
899                                         Mat4Invert(imat, ob->obmat);
900                                         Mat4CpyMat4(tempmat, mat);
901                                         Mat4MulMat4(mat, tempmat, imat);
902                                 }
903                                 /* pose to local */
904                                 if (to == CONSTRAINT_SPACE_LOCAL) {
905                                         /* call self with slightly different values */
906                                         constraint_mat_convertspace(ob, pchan, mat, CONSTRAINT_SPACE_POSE, to);
907                                 }
908                                 /* pose to local + parent */
909                                 else if (to == CONSTRAINT_SPACE_PARLOCAL) {
910                                         /* call self with slightly different values */
911                                         constraint_mat_convertspace(ob, pchan, mat, CONSTRAINT_SPACE_POSE, to);
912                                 }
913                         }
914                                 break;
915                         case CONSTRAINT_SPACE_POSE:     /* ---------- FROM POSESPACE ---------- */
916                         {
917                                 /* pose to world */
918                                 if (to == CONSTRAINT_SPACE_WORLD) {
919                                         Mat4CpyMat4(tempmat, mat);
920                                         Mat4MulMat4(mat, tempmat, ob->obmat);
921                                 }
922                                 /* pose to local */
923                                 else if (to == CONSTRAINT_SPACE_LOCAL) {
924                                         if (pchan->bone) {
925                                                 if (pchan->parent && pchan->parent->bone) {
926                                                         float offs_bone[4][4];
927                                                                 
928                                                         /* construct offs_bone the same way it is done in armature.c */
929                                                         Mat4CpyMat3(offs_bone, pchan->bone->bone_mat);
930                                                         VECCOPY(offs_bone[3], pchan->bone->head);
931                                                         offs_bone[3][1]+= pchan->parent->bone->length;
932                                                         
933                                                         if (pchan->bone->flag & BONE_HINGE) {
934                                                                 /* pose_mat = par_pose-space_location * chan_mat */
935                                                                 float tmat[4][4];
936                                                                 
937                                                                 /* the rotation of the parent restposition */
938                                                                 Mat4CpyMat4(tmat, pchan->parent->bone->arm_mat);
939                                                                 
940                                                                 /* the location of actual parent transform */
941                                                                 VECCOPY(tmat[3], offs_bone[3]);
942                                                                 offs_bone[3][0]= offs_bone[3][1]= offs_bone[3][2]= 0.0f;
943                                                                 Mat4MulVecfl(pchan->parent->pose_mat, tmat[3]);
944                                                                 
945                                                                 Mat4MulMat4(diff_mat, offs_bone, tmat);
946                                                                 Mat4Invert(imat, diff_mat);
947                                                         }
948                                                         else {
949                                                                 /* pose_mat = par_pose_mat * bone_mat * chan_mat */
950                                                                 Mat4MulMat4(diff_mat, pchan->parent->pose_mat, offs_bone);
951                                                                 Mat4Invert(imat, diff_mat);
952                                                         }
953                                                 }
954                                                 else {
955                                                         /* pose_mat = chan_mat * arm_mat */
956                                                         Mat4Invert(imat, pchan->bone->arm_mat);
957                                                 }
958                                                 
959                                                 Mat4CpyMat4(tempmat, mat);
960                                                 Mat4MulMat4(mat, tempmat, imat);
961                                         }
962                                 }
963                                 /* pose to local with parent */
964                                 else if (to == CONSTRAINT_SPACE_PARLOCAL) {
965                                         if (pchan->bone) {
966                                                 Mat4Invert(imat, pchan->bone->arm_mat);
967                                                 Mat4CpyMat4(tempmat, mat);
968                                                 Mat4MulMat4(mat, tempmat, imat);
969                                         }
970                                 }
971                         }
972                                 break;
973                         case CONSTRAINT_SPACE_LOCAL: /* ------------ FROM LOCALSPACE --------- */
974                         {
975                                 /* local to pose */
976                                 if (to==CONSTRAINT_SPACE_POSE || to==CONSTRAINT_SPACE_WORLD) {
977                                         /* do inverse procedure that was done for pose to local */
978                                         if (pchan->bone) {
979                                                 /* we need the posespace_matrix = local_matrix + (parent_posespace_matrix + restpos) */                                         
980                                                 if (pchan->parent) {
981                                                         float offs_bone[4][4];
982                                                                 
983                                                         /* construct offs_bone the same way it is done in armature.c */
984                                                         Mat4CpyMat3(offs_bone, pchan->bone->bone_mat);
985                                                         VECCOPY(offs_bone[3], pchan->bone->head);
986                                                         offs_bone[3][1]+= pchan->parent->bone->length;
987                                                         
988                                                         Mat4MulMat4(diff_mat, offs_bone, pchan->parent->pose_mat);
989                                                         Mat4CpyMat4(tempmat, mat);
990                                                         Mat4MulMat4(mat, tempmat, diff_mat);
991                                                 }
992                                                 else {
993                                                         Mat4CpyMat4(diff_mat, pchan->bone->arm_mat);
994                                                         
995                                                         Mat4CpyMat4(tempmat, mat);
996                                                         Mat4MulMat4(mat, tempmat, diff_mat);
997                                                 }
998                                         }
999                                 }
1000                                 /* local to world */
1001                                 if (to == CONSTRAINT_SPACE_WORLD) {
1002                                         /* call self with slightly different values */
1003                                         constraint_mat_convertspace(ob, pchan, mat, CONSTRAINT_SPACE_POSE, to);
1004                                 }
1005                         }
1006                                 break;
1007                         case CONSTRAINT_SPACE_PARLOCAL: /* -------------- FROM LOCAL WITH PARENT ---------- */
1008                         {
1009                                 /* local to pose */
1010                                 if (to==CONSTRAINT_SPACE_POSE || to==CONSTRAINT_SPACE_WORLD) {
1011                                         if (pchan->bone) {                                      
1012                                                 Mat4CpyMat4(diff_mat, pchan->bone->arm_mat);
1013                                                 Mat4CpyMat4(tempmat, mat);
1014                                                 Mat4MulMat4(mat, diff_mat, tempmat);
1015                                         }
1016                                 }
1017                                 /* local to world */
1018                                 if (to == CONSTRAINT_SPACE_WORLD) {
1019                                         /* call self with slightly different values */
1020                                         constraint_mat_convertspace(ob, pchan, mat, CONSTRAINT_SPACE_POSE, to);
1021                                 }
1022                         }
1023                                 break;
1024                 }
1025         }
1026         else {
1027                 /* objects */
1028                 if (from==CONSTRAINT_SPACE_WORLD && to==CONSTRAINT_SPACE_LOCAL) {
1029                         /* check if object has a parent - otherwise this won't work */
1030                         if (ob->parent) {
1031                                 /* 'subtract' parent's effects from owner */
1032                                 Mat4MulMat4(diff_mat, ob->parentinv, ob->parent->obmat);
1033                                 Mat4Invert(imat, diff_mat);
1034                                 Mat4CpyMat4(tempmat, mat);
1035                                 Mat4MulMat4(mat, tempmat, imat);
1036                         }
1037                 }
1038                 else if (from==CONSTRAINT_SPACE_LOCAL && to==CONSTRAINT_SPACE_WORLD) {
1039                         /* check that object has a parent - otherwise this won't work */
1040                         if (ob->parent) {
1041                                 /* 'add' parent's effect back to owner */
1042                                 Mat4CpyMat4(tempmat, mat);
1043                                 Mat4MulMat4(diff_mat, ob->parentinv, ob->parent->obmat);
1044                                 Mat4MulMat4(mat, tempmat, diff_mat);
1045                         }
1046                 }
1047         }
1048 }
1049
1050 /* ------------------------------- Target ---------------------------- */
1051
1052 /* generic function to get the appropriate matrix for most target cases */
1053 /* The cases where the target can be object data have not been implemented */
1054 // TODO: add 'space' handling stuff here! 
1055 static void constraint_target_to_mat4 (Object *ob, const char *substring, float mat[][4], short from, short to)
1056 {
1057         /*      Case OBJECT */
1058         if (!strlen(substring)) {
1059                 Mat4CpyMat4(mat, ob->obmat);
1060                 constraint_mat_convertspace(ob, NULL, mat, from, to);
1061         }
1062         /*      Case VERTEXGROUP */
1063         else if (ELEM(ob->type, OB_MESH, OB_LATTICE)) {
1064                 /* devise a matrix from the data in the vertexgroup */
1065                 /* TODO: will be handled in other files */
1066         }
1067         /*      Case BONE */
1068         else {
1069                 bPoseChannel *pchan;
1070                 
1071                 pchan = get_pose_channel(ob->pose, substring);
1072                 if (pchan) {
1073                         /* Multiply the PoseSpace accumulation/final matrix for this
1074                          * PoseChannel by the Armature Object's Matrix to get a worldspace
1075                          * matrix.
1076                          */
1077                         Mat4MulMat4(mat, pchan->pose_mat, ob->obmat);
1078                 } 
1079                 else
1080                         Mat4CpyMat4(mat, ob->obmat);
1081                         
1082                 /* convert matrix space as required */
1083                 constraint_mat_convertspace(ob, pchan, mat, from, to);
1084         }
1085 }
1086
1087
1088 /* stupid little cross product function, 0:x, 1:y, 2:z axes */
1089 static int basis_cross(int n, int m)
1090 {
1091         if(n-m == 1) return 1;
1092         if(n-m == -1) return -1;
1093         if(n-m == 2) return -1;
1094         if(n-m == -2) return 1;
1095         else return 0;
1096 }
1097
1098 static void vectomat(float *vec, float *target_up, short axis, short upflag, short flags, float m[][3])
1099 {
1100         float n[3];
1101         float u[3]; /* vector specifying the up axis */
1102         float proj[3];
1103         float right[3];
1104         float neg = -1;
1105         int right_index;
1106         
1107         VecCopyf(n, vec);
1108         if(Normalize(n) == 0.0) { 
1109                 n[0] = 0.0;
1110                 n[1] = 0.0;
1111                 n[2] = 1.0;
1112         }
1113         if(axis > 2) axis -= 3;
1114         else VecMulf(n,-1);
1115
1116         /* n specifies the transformation of the track axis */
1117
1118         if(flags & TARGET_Z_UP) { 
1119                 /* target Z axis is the global up axis */
1120                 u[0] = target_up[0];
1121                 u[1] = target_up[1];
1122                 u[2] = target_up[2];
1123         }
1124         else { 
1125                 /* world Z axis is the global up axis */
1126                 u[0] = 0;
1127                 u[1] = 0;
1128                 u[2] = 1;
1129         }
1130
1131         /* project the up vector onto the plane specified by n */
1132         Projf(proj, u, n); /* first u onto n... */
1133         VecSubf(proj, u, proj); /* then onto the plane */
1134         /* proj specifies the transformation of the up axis */
1135
1136         if(Normalize(proj) == 0.0) { /* degenerate projection */
1137                 proj[0] = 0.0;
1138                 proj[1] = 1.0;
1139                 proj[2] = 0.0;
1140         }
1141
1142         /* Normalized cross product of n and proj specifies transformation of the right axis */
1143         Crossf(right, proj, n);
1144         Normalize(right);
1145
1146         if(axis != upflag) {
1147                 right_index = 3 - axis - upflag;
1148                 neg = (float) basis_cross(axis, upflag);
1149
1150                 /* account for up direction, track direction */
1151                 m[right_index][0] = neg * right[0];
1152                 m[right_index][1] = neg * right[1];
1153                 m[right_index][2] = neg * right[2];
1154
1155                 m[upflag][0] = proj[0];
1156                 m[upflag][1] = proj[1];
1157                 m[upflag][2] = proj[2];
1158
1159                 m[axis][0] = n[0];
1160                 m[axis][1] = n[1];
1161                 m[axis][2] = n[2];
1162         }
1163         /* identity matrix - don't do anything if the two axes are the same */
1164         else {
1165                 m[0][0]= m[1][1]= m[2][2]= 1.0;
1166                 m[0][1]= m[0][2]= m[0][3]= 0.0;
1167                 m[1][0]= m[1][2]= m[1][3]= 0.0;
1168                 m[2][0]= m[2][1]= m[2][3]= 0.0;
1169         }
1170 }
1171
1172
1173 /* called during solve_constraints */
1174 /* also for make_parent, to find correct inverse of "follow path" */
1175 /* warning: ownerdata is PoseChannel or Object */
1176 /* ctime is global time, uncorrected for local bsystem_time */
1177 short get_constraint_target_matrix (bConstraint *con, short ownertype, void *ownerdata, float mat[][4], float ctime)
1178 {
1179         short valid=0;
1180
1181         switch (con->type) {
1182         case CONSTRAINT_TYPE_NULL:
1183                 {
1184                         Mat4One(mat);
1185                 }
1186                 break;
1187         case CONSTRAINT_TYPE_ACTION:
1188                 {
1189                         if (ownertype == TARGET_BONE) {
1190                                 extern void chan_calc_mat(bPoseChannel *chan);
1191                                 bActionConstraint *data = (bActionConstraint*)con->data;
1192                                 bPose *pose;
1193                                 bPoseChannel *pchan, *tchan;
1194                                 float tempmat[4][4], vec[3];
1195                                 float s, t;
1196                                 short axis;
1197                                 
1198                                 /* initialise return matrix */
1199                                 Mat4One(mat);
1200                                 
1201                                 /* only continue if there is a target  */
1202                                 if (data->tar==NULL) return 0;
1203                                 
1204                                 /* get the transform matrix of the target */
1205                                 constraint_target_to_mat4(data->tar, data->subtarget, tempmat, CONSTRAINT_SPACE_WORLD, con->tarspace); // FIXME: change these spaces 
1206                                 
1207                                 /* determine where in transform range target is */
1208                                 /* data->type is mapped as follows for backwards compatability:
1209                                  *      00,01,02        - rotation (it used to be like this)
1210                                  *      10,11,12        - scaling
1211                                  *      20,21,22        - location
1212                                  */
1213                                 if (data->type < 10) {
1214                                         /* extract rotation (is in whatever space target should be in) */
1215                                         Mat4ToEul(tempmat, vec);
1216                                         vec[0] *= (float)(180.0/M_PI);
1217                                         vec[1] *= (float)(180.0/M_PI);
1218                                         vec[2] *= (float)(180.0/M_PI);
1219                                         axis= data->type;
1220                                 }
1221                                 else if (data->type < 20) {
1222                                         /* extract scaling (is in whatever space target should be in) */
1223                                         Mat4ToSize(tempmat, vec);
1224                                         axis= data->type - 10;
1225                                 }
1226                                 else {
1227                                         /* extract location */
1228                                         VECCOPY(vec, tempmat[3]);
1229                                         axis= data->type - 20;
1230                                 }
1231                                 
1232                                 /* Target defines the animation */
1233                                 s = (vec[axis]-data->min) / (data->max-data->min);
1234                                 CLAMP(s, 0, 1);
1235                                 t = ( s * (data->end-data->start)) + data->start;
1236                                 
1237                                 /* Get the appropriate information from the action, we make temp pose */
1238                                 pose = MEM_callocN(sizeof(bPose), "pose");
1239                                 
1240                                 pchan = ownerdata;
1241                                 tchan= verify_pose_channel(pose, pchan->name);
1242                                 extract_pose_from_action(pose, data->act, t);
1243                                 
1244                                 chan_calc_mat(tchan);
1245                                 
1246                                 Mat4CpyMat4(mat, tchan->chan_mat);
1247                                 
1248                                 /* Clean up */
1249                                 free_pose_channels(pose);
1250                                 MEM_freeN(pose);
1251                         }
1252                 }
1253                 break;
1254         case CONSTRAINT_TYPE_LOCLIKE:
1255                 {
1256                         bLocateLikeConstraint *data = (bLocateLikeConstraint*)con->data;
1257                         Object *ob= data->tar;
1258                         
1259                         if (data->tar) {
1260                                 if (strlen(data->subtarget)) {
1261                                         bPoseChannel *pchan;
1262                                         float tmat[4][4];
1263                                         
1264                                         pchan = get_pose_channel(ob->pose, data->subtarget);
1265                                         if (pchan) {
1266                                                 Mat4CpyMat4(tmat, pchan->pose_mat);
1267                                                 
1268                                                 if (data->flag & LOCLIKE_TIP) { 
1269                                                         VECCOPY(tmat[3], pchan->pose_tail);
1270                                                 }
1271                                                         
1272                                                 Mat4MulMat4(mat, tmat, ob->obmat);
1273                                         }
1274                                         else 
1275                                                 Mat4CpyMat4(mat, ob->obmat);
1276                                                 
1277                                         /* convert matrix space as required  */
1278                                         constraint_mat_convertspace(ob, pchan, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1279                                 }
1280                                 else {
1281                                         Mat4CpyMat4(mat, ob->obmat);
1282                                         
1283                                         /* convert matrix space as required */
1284                                         constraint_mat_convertspace(ob, NULL, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1285                                 }
1286                                 valid=1;
1287                         }
1288                         else
1289                                 Mat4One(mat);
1290                 } 
1291                 break;
1292         case CONSTRAINT_TYPE_ROTLIKE:
1293                 {
1294                         bRotateLikeConstraint *data;
1295                         data = (bRotateLikeConstraint*)con->data;
1296                         
1297                         if (data->tar) {
1298                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1299                                 valid=1;
1300                         }
1301                         else
1302                                 Mat4One(mat);
1303                 } 
1304                 break;
1305         case CONSTRAINT_TYPE_SIZELIKE:
1306                 {
1307                         bSizeLikeConstraint *data;
1308                         data = (bSizeLikeConstraint*)con->data;
1309                         
1310                         if (data->tar) {
1311                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1312                                 valid=1;
1313                         }
1314                         else
1315                                 Mat4One(mat);
1316                 } 
1317                 break;
1318         case CONSTRAINT_TYPE_MINMAX:
1319                 {
1320                         bMinMaxConstraint *data = (bMinMaxConstraint*)con->data;
1321                         
1322                         if (data->tar) {
1323                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1324                                 valid=1;
1325                         }
1326                         else
1327                                 Mat4One(mat);
1328                 } 
1329                 break;
1330         case CONSTRAINT_TYPE_TRACKTO:
1331                 {
1332                         bTrackToConstraint *data;
1333                         data = (bTrackToConstraint*)con->data;
1334                         
1335                         if (data->tar) {
1336                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1337                                 valid=1;
1338                         }
1339                         else
1340                                 Mat4One (mat);
1341                 }
1342                 break;
1343         case CONSTRAINT_TYPE_KINEMATIC:
1344                 {
1345                         bKinematicConstraint *data;
1346                         data = (bKinematicConstraint*)con->data;
1347                         
1348                         if (data->tar) {
1349                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1350                                 valid=1;
1351                         }
1352                         else if (data->flag & CONSTRAINT_IK_AUTO) {
1353                                 Object *ob= (Object *)ownerdata;
1354                                 
1355                                 if (ob==NULL)
1356                                         Mat4One(mat);
1357                                 else {
1358                                         float vec[3];
1359                                         /* move grabtarget into world space */
1360                                         VECCOPY(vec, data->grabtarget);
1361                                         Mat4MulVecfl(ob->obmat, vec);
1362                                         Mat4CpyMat4(mat, ob->obmat);
1363                                         VECCOPY(mat[3], vec);
1364                                 }
1365                         }
1366                         else
1367                                 Mat4One(mat);
1368                 } 
1369                 break;
1370         case CONSTRAINT_TYPE_LOCKTRACK:
1371                 {
1372                         bLockTrackConstraint *data;
1373                         data = (bLockTrackConstraint*)con->data;
1374                         
1375                         if (data->tar) {
1376                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace); 
1377                                 valid=1;
1378                         }
1379                         else
1380                                 Mat4One(mat);
1381                 } 
1382                 break;
1383         case CONSTRAINT_TYPE_FOLLOWPATH:
1384                 {
1385                         bFollowPathConstraint *data;
1386                         data = (bFollowPathConstraint*)con->data;
1387                         
1388                         if (data->tar) {
1389                                 Curve *cu;
1390                                 float q[4], vec[4], dir[3], *quat, x1;
1391                                 float totmat[4][4];
1392                                 float curvetime;
1393                                 
1394                                 Mat4One(totmat);
1395                                 Mat4One(mat);
1396                                 
1397                                 cu= data->tar->data;
1398                                 
1399                                 /* note: when creating constraints that follow path, the curve gets the CU_PATH set now,
1400                                         currently for paths to work it needs to go through the bevlist/displist system (ton) */
1401                                 
1402                                 if (cu->path==NULL || cu->path->data==NULL) /* only happens on reload file, but violates depsgraph still... fix! */
1403                                         makeDispListCurveTypes(data->tar, 0);
1404                                 if (cu->path && cu->path->data) {
1405                                         curvetime= bsystem_time(data->tar, data->tar->parent, (float)ctime, 0.0) - data->offset;
1406                                         
1407                                         if (calc_ipo_spec(cu->ipo, CU_SPEED, &curvetime)==0) {
1408                                                 curvetime /= cu->pathlen;
1409                                                 CLAMP(curvetime, 0.0, 1.0);
1410                                         }
1411                                         
1412                                         if (where_on_path(data->tar, curvetime, vec, dir) ) {
1413                                                 if (data->followflag) {
1414                                                         quat= vectoquat(dir, (short) data->trackflag, (short) data->upflag);
1415                                                         
1416                                                         Normalize(dir);
1417                                                         q[0]= (float)cos(0.5*vec[3]);
1418                                                         x1= (float)sin(0.5*vec[3]);
1419                                                         q[1]= -x1*dir[0];
1420                                                         q[2]= -x1*dir[1];
1421                                                         q[3]= -x1*dir[2];
1422                                                         QuatMul(quat, q, quat);
1423                                                         
1424                                                         QuatToMat4(quat, totmat);
1425                                                 }
1426                                                 VECCOPY(totmat[3], vec);
1427                                                 
1428                                                 Mat4MulSerie(mat, data->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
1429                                         }
1430                                 }
1431                                 valid=1;
1432                         }
1433                         else
1434                                 Mat4One(mat);
1435                 }
1436                 break;
1437         case CONSTRAINT_TYPE_STRETCHTO:
1438                 {
1439                         bStretchToConstraint *data;
1440                         data = (bStretchToConstraint*)con->data;
1441                         
1442                         if (data->tar) {
1443                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1444                                 valid = 1;
1445                         }
1446                         else
1447                                 Mat4One(mat);
1448                 }
1449                 break;
1450         case CONSTRAINT_TYPE_PYTHON:
1451                 {
1452                         bPythonConstraint *data;
1453                         data = (bPythonConstraint*)con->data;
1454                         
1455                         /* special exception for curves - depsgraph issues */
1456                         if (data->tar && data->tar->type == OB_CURVE) {
1457                                 Curve *cu= data->tar->data;
1458                                 
1459                                 /* this check is to make sure curve objects get updated on file load correctly.*/
1460                                 if (cu->path==NULL || cu->path->data==NULL) /* only happens on reload file, but violates depsgraph still... fix! */
1461                                         makeDispListCurveTypes(data->tar, 0);                           
1462                         }
1463                         
1464                         /* if the script doesn't set the target matrix for any reason, fall back to standard methods */
1465                         if (BPY_pyconstraint_targets(data, mat) < 1) {
1466                                 if (data->tar) {
1467                                         constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1468                                         valid = 1;
1469                                 }
1470                                 else
1471                                         Mat4One(mat);
1472                         }
1473                 }
1474                 break;
1475         case CONSTRAINT_TYPE_CLAMPTO:
1476                 {
1477                         bClampToConstraint *data;
1478                         data = (bClampToConstraint*)con->data;
1479                         
1480                         if (data->tar) {
1481                                 Curve *cu= data->tar->data;
1482                                 
1483                                 /* note; when creating constraints that follow path, the curve gets the CU_PATH set now,
1484                                         currently for paths to work it needs to go through the bevlist/displist system (ton) */
1485                                 
1486                                 if (cu->path==NULL || cu->path->data==NULL) /* only happens on reload file, but violates depsgraph still... fix! */
1487                                         makeDispListCurveTypes(data->tar, 0);
1488                                 
1489                                 valid = 1;
1490                         }
1491                         
1492                         Mat4One(mat);
1493                 }
1494                 break;
1495         case CONSTRAINT_TYPE_CHILDOF:
1496                 {
1497                         bChildOfConstraint *data;
1498                         data= (bChildOfConstraint *)con->data;
1499                         
1500                         if (data->tar) {
1501                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, CONSTRAINT_SPACE_WORLD, con->tarspace);
1502                                 valid = 1;
1503                         }
1504                         else
1505                                 Mat4One(mat);
1506                 }
1507                 break;
1508
1509         default:
1510                 Mat4One(mat);
1511                 break;
1512         }
1513
1514         return valid;
1515 }
1516
1517 /* ---------------------------------------------- Constraint Evaluation ------------------------------------------------- */
1518
1519 /* This is only called during solve_constraints to solve a particular constraint.
1520  * It works on ownermat, and uses targetmat to help accomplish its tasks.
1521  */
1522 static void evaluate_constraint (bConstraint *constraint, float ownermat[][4], float targetmat[][4])
1523 {
1524         if (constraint == NULL || constraint->data == NULL)
1525                 return;
1526                 
1527         switch (constraint->type) {
1528         case CONSTRAINT_TYPE_NULL:
1529         case CONSTRAINT_TYPE_KINEMATIC: /* removed */
1530                 break;
1531         case CONSTRAINT_TYPE_PYTHON:
1532                 {
1533                         bPythonConstraint *data;
1534                         
1535                         data = constraint->data;
1536                         BPY_pyconstraint_eval(data, ownermat, targetmat);
1537                 } 
1538                 break;
1539         case CONSTRAINT_TYPE_ACTION:
1540                 {
1541                         bActionConstraint *data;
1542                         float temp[4][4];
1543                         
1544                         data = constraint->data;
1545                         Mat4CpyMat4(temp, ownermat);
1546                         
1547                         Mat4MulMat4(ownermat, targetmat, temp);
1548                 }
1549                 break;
1550         case CONSTRAINT_TYPE_LOCLIKE:
1551                 {
1552                         bLocateLikeConstraint *data;
1553                         float offset[3] = {0.0f, 0.0f, 0.0f};
1554
1555                         data = constraint->data;
1556                         
1557                         if (data->flag & LOCLIKE_OFFSET)
1558                                 VECCOPY(offset, ownermat[3]);
1559                         
1560                         if (data->flag & LOCLIKE_X) {
1561                                 ownermat[3][0] = targetmat[3][0];
1562                                 
1563                                 if(data->flag & LOCLIKE_X_INVERT) ownermat[3][0] *= -1;
1564                                 ownermat[3][0] += offset[0];
1565                         }
1566                         if (data->flag & LOCLIKE_Y) {
1567                                 ownermat[3][1] = targetmat[3][1];
1568                                 
1569                                 if(data->flag & LOCLIKE_Y_INVERT) ownermat[3][1] *= -1;
1570                                 ownermat[3][1] += offset[1];
1571                         }
1572                         if (data->flag & LOCLIKE_Z) {
1573                                 ownermat[3][2] = targetmat[3][2];
1574                                 
1575                                 if(data->flag & LOCLIKE_Z_INVERT) ownermat[3][2] *= -1;
1576                                 ownermat[3][2] += offset[2];
1577                         }
1578                 }
1579                 break;
1580         case CONSTRAINT_TYPE_ROTLIKE:
1581                 {
1582                         bRotateLikeConstraint *data;
1583                         float   loc[3];
1584                         float   eul[3], obeul[3];
1585                         float   size[3];
1586                         short changed= 0;
1587                         
1588                         data = constraint->data;
1589                         
1590                         VECCOPY(loc, ownermat[3]);
1591                         Mat4ToSize(ownermat, size);
1592                         
1593                         Mat4ToEul(targetmat, eul);
1594                         Mat4ToEul(ownermat, obeul);
1595                         
1596                         if ((data->flag & ROTLIKE_X)==0) {
1597                                 eul[0] = obeul[0];
1598                                 changed = 1;
1599                         }
1600                         else if (data->flag & ROTLIKE_X_INVERT) {
1601                                 eul[0] *= -1;
1602                                 changed = 1;
1603                         }       
1604                         if ((data->flag & ROTLIKE_Y)==0) {
1605                                 eul[1] = obeul[1];
1606                                 changed = 1;
1607                         }
1608                         else if (data->flag & ROTLIKE_Y_INVERT) {
1609                                 eul[1] *= -1;
1610                                 changed = 1;
1611                         }
1612                         if ((data->flag & ROTLIKE_Z)==0) {
1613                                 eul[2] = obeul[2];
1614                                 changed = 1;
1615                         }
1616                         else if (data->flag & ROTLIKE_Z_INVERT) {
1617                                 eul[2] *= -1;
1618                                 changed = 1;
1619                         }
1620                         
1621                         
1622                         if (changed) {
1623                                 compatible_eul(eul, obeul);
1624                                 LocEulSizeToMat4(ownermat, loc, eul, size);
1625                         }
1626                         else {
1627                                 float quat[4];
1628                                 
1629                                 Mat4ToQuat(targetmat, quat);
1630                                 LocQuatSizeToMat4(ownermat, loc, quat, size);
1631                         }       
1632                 }
1633                 break;
1634         case CONSTRAINT_TYPE_SIZELIKE:
1635                 {
1636                         bSizeLikeConstraint *data;
1637                         float obsize[3], size[3];
1638                         
1639                         data = constraint->data;
1640  
1641                         Mat4ToSize(targetmat, size);
1642                         Mat4ToSize(ownermat, obsize);
1643                         
1644                         if ((data->flag & SIZELIKE_X) && obsize[0] != 0)
1645                                 VecMulf(ownermat[0], size[0] / obsize[0]);
1646                         if ((data->flag & SIZELIKE_Y) && obsize[1] != 0)
1647                                 VecMulf(ownermat[1], size[1] / obsize[1]);
1648                         if ((data->flag & SIZELIKE_Z) && obsize[2] != 0)
1649                                 VecMulf(ownermat[2], size[2] / obsize[2]);
1650                 }
1651                 break;
1652         case CONSTRAINT_TYPE_MINMAX:
1653                 {
1654                         bMinMaxConstraint *data;
1655                         float obmat[4][4], imat[4][4], tarmat[4][4], tmat[4][4];
1656                         float val1, val2;
1657                         int index;
1658                         
1659                         data = constraint->data;
1660                         
1661                         Mat4CpyMat4(obmat, ownermat);
1662                         Mat4CpyMat4(tarmat, targetmat);
1663                         
1664                         if (data->flag & MINMAX_USEROT) {
1665                                 /* take rotation of target into account by doing the transaction in target's localspace */
1666                                 Mat4Invert(imat, tarmat);
1667                                 Mat4MulMat4(tmat, obmat, imat);
1668                                 Mat4CpyMat4(obmat, tmat);
1669                                 Mat4One(tarmat);
1670                         }
1671                         
1672                         switch (data->minmaxflag) {
1673                         case TRACK_Z:
1674                                 val1 = tarmat[3][2];
1675                                 val2 = obmat[3][2]-data->offset;
1676                                 index = 2;
1677                                 break;
1678                         case TRACK_Y:
1679                                 val1 = tarmat[3][1];
1680                                 val2 = obmat[3][1]-data->offset;
1681                                 index = 1;
1682                                 break;
1683                         case TRACK_X:
1684                                 val1 = tarmat[3][0];
1685                                 val2 = obmat[3][0]-data->offset;
1686                                 index = 0;
1687                                 break;
1688                         case TRACK_nZ:
1689                                 val2 = tarmat[3][2];
1690                                 val1 = obmat[3][2]-data->offset;
1691                                 index = 2;
1692                                 break;
1693                         case TRACK_nY:
1694                                 val2 = tarmat[3][1];
1695                                 val1 = obmat[3][1]-data->offset;
1696                                 index = 1;
1697                                 break;
1698                         case TRACK_nX:
1699                                 val2 = tarmat[3][0];
1700                                 val1 = obmat[3][0]-data->offset;
1701                                 index = 0;
1702                                 break;
1703                         default:
1704                                 return;
1705                         }
1706                         
1707                         if (val1 > val2) {
1708                                 obmat[3][index] = tarmat[3][index] + data->offset;
1709                                 if (data->flag & MINMAX_STICKY) {
1710                                         if (data->flag & MINMAX_STUCK) {
1711                                                 VECCOPY(obmat[3], data->cache);
1712                                         } 
1713                                         else {
1714                                                 VECCOPY(data->cache, obmat[3]);
1715                                                 data->flag |= MINMAX_STUCK;
1716                                         }
1717                                 }
1718                                 if (data->flag & MINMAX_USEROT) {
1719                                         /* get out of localspace */
1720                                         Mat4MulMat4(tmat, obmat, targetmat);
1721                                         Mat4CpyMat4(ownermat, tmat);
1722                                 } 
1723                                 else {                  
1724                                         VECCOPY(ownermat[3], obmat[3]);
1725                                 }
1726                         } 
1727                         else {
1728                                 data->flag &= ~MINMAX_STUCK;
1729                         }
1730                 }
1731                 break;
1732         case CONSTRAINT_TYPE_TRACKTO:
1733                 {
1734                         bTrackToConstraint *data;
1735                         float size[3], vec[3];
1736                         float totmat[3][3];
1737                         float tmat[4][4];
1738
1739                         data = constraint->data;                        
1740                         
1741                         if (data->tar) {
1742                                 /* Get size property, since ob->size is only the object's own relative size, not its global one */
1743                                 Mat4ToSize (ownermat, size);
1744                                 
1745                                 /* Clear the object's rotation */       
1746                                 ownermat[0][0]=size[0];
1747                                 ownermat[0][1]=0;
1748                                 ownermat[0][2]=0;
1749                                 ownermat[1][0]=0;
1750                                 ownermat[1][1]=size[1];
1751                                 ownermat[1][2]=0;
1752                                 ownermat[2][0]=0;
1753                                 ownermat[2][1]=0;
1754                                 ownermat[2][2]=size[2];
1755                                 
1756                                 VecSubf(vec, ownermat[3], targetmat[3]);
1757                                 vectomat(vec, ownermat[2], 
1758                                                 (short)data->reserved1, (short)data->reserved2, 
1759                                                 data->flags, totmat);
1760                                 
1761                                 Mat4CpyMat4(tmat, ownermat);
1762                                 Mat4MulMat34(ownermat, totmat, tmat);
1763                         }
1764                 }
1765                 break;
1766         case CONSTRAINT_TYPE_LOCKTRACK:
1767                 {
1768                         bLockTrackConstraint *data;
1769                         float vec[3],vec2[3];
1770                         float totmat[3][3];
1771                         float tmpmat[3][3];
1772                         float invmat[3][3];
1773                         float tmat[4][4];
1774                         float mdet;
1775
1776                         data = constraint->data;                        
1777                         
1778                         if (data->tar) {
1779                                 /* Vector object -> target */
1780                                 VecSubf(vec, targetmat[3], ownermat[3]);
1781                                 switch (data->lockflag){
1782                                 case LOCK_X: /* LOCK X */
1783                                 {
1784                                         switch (data->trackflag) {
1785                                                 case TRACK_Y: /* LOCK X TRACK Y */
1786                                                 {
1787                                                         /* Projection of Vector on the plane */
1788                                                         Projf(vec2, vec, ownermat[0]);
1789                                                         VecSubf(totmat[1], vec, vec2);
1790                                                         Normalize(totmat[1]);
1791                                                         
1792                                                         /* the x axis is fixed */
1793                                                         totmat[0][0] = ownermat[0][0];
1794                                                         totmat[0][1] = ownermat[0][1];
1795                                                         totmat[0][2] = ownermat[0][2];
1796                                                         Normalize(totmat[0]);
1797                                                         
1798                                                         /* the z axis gets mapped onto a third orthogonal vector */
1799                                                         Crossf(totmat[2], totmat[0], totmat[1]);
1800                                                 }
1801                                                         break;
1802                                                 case TRACK_Z: /* LOCK X TRACK Z */
1803                                                 {
1804                                                         /* Projection of Vector on the plane */
1805                                                         Projf(vec2, vec, ownermat[0]);
1806                                                         VecSubf(totmat[2], vec, vec2);
1807                                                         Normalize(totmat[2]);
1808
1809                                                         /* the x axis is fixed */
1810                                                         totmat[0][0] = ownermat[0][0];
1811                                                         totmat[0][1] = ownermat[0][1];
1812                                                         totmat[0][2] = ownermat[0][2];
1813                                                         Normalize(totmat[0]);
1814                                         
1815                                                         /* the z axis gets mapped onto a third orthogonal vector */
1816                                                         Crossf(totmat[1], totmat[2], totmat[0]);
1817                                                 }
1818                                                         break;
1819                                                 case TRACK_nY: /* LOCK X TRACK -Y */
1820                                                 {
1821                                                         /* Projection of Vector on the plane */
1822                                                         Projf(vec2, vec, ownermat[0]);
1823                                                         VecSubf(totmat[1], vec, vec2);
1824                                                         Normalize(totmat[1]);
1825                                                         VecMulf(totmat[1],-1);
1826                                                         
1827                                                         /* the x axis is fixed */
1828                                                         totmat[0][0] = ownermat[0][0];
1829                                                         totmat[0][1] = ownermat[0][1];
1830                                                         totmat[0][2] = ownermat[0][2];
1831                                                         Normalize(totmat[0]);
1832                                                         
1833                                                         /* the z axis gets mapped onto a third orthogonal vector */
1834                                                         Crossf(totmat[2], totmat[0], totmat[1]);
1835                                                 }
1836                                                         break;
1837                                                 case TRACK_nZ: /* LOCK X TRACK -Z */
1838                                                 {
1839                                                         /* Projection of Vector on the plane */
1840                                                         Projf(vec2, vec, ownermat[0]);
1841                                                         VecSubf(totmat[2], vec, vec2);
1842                                                         Normalize(totmat[2]);
1843                                                         VecMulf(totmat[2],-1);
1844                                                                 
1845                                                         /* the x axis is fixed */
1846                                                         totmat[0][0] = ownermat[0][0];
1847                                                         totmat[0][1] = ownermat[0][1];
1848                                                         totmat[0][2] = ownermat[0][2];
1849                                                         Normalize(totmat[0]);
1850                                                                 
1851                                                         /* the z axis gets mapped onto a third orthogonal vector */
1852                                                         Crossf(totmat[1], totmat[2], totmat[0]);
1853                                                 }
1854                                                         break;
1855                                                 default:
1856                                                 {
1857                                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1858                                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1859                                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1860                                                 }
1861                                                         break;
1862                                         }
1863                                 }
1864                                         break;
1865                                 case LOCK_Y: /* LOCK Y */
1866                                 {
1867                                         switch (data->trackflag) {
1868                                                 case TRACK_X: /* LOCK Y TRACK X */
1869                                                 {
1870                                                         /* Projection of Vector on the plane */
1871                                                         Projf(vec2, vec, ownermat[1]);
1872                                                         VecSubf(totmat[0], vec, vec2);
1873                                                         Normalize(totmat[0]);
1874                                                         
1875                                                         /* the y axis is fixed */
1876                                                         totmat[1][0] = ownermat[1][0];
1877                                                         totmat[1][1] = ownermat[1][1];
1878                                                         totmat[1][2] = ownermat[1][2];
1879                                                         Normalize(totmat[1]);
1880                                                         
1881                                                         /* the z axis gets mapped onto a third orthogonal vector */
1882                                                         Crossf(totmat[2], totmat[0], totmat[1]);
1883                                                 }
1884                                                         break;
1885                                                 case TRACK_Z: /* LOCK Y TRACK Z */
1886                                                 {
1887                                                         /* Projection of Vector on the plane */
1888                                                         Projf(vec2, vec, ownermat[1]);
1889                                                         VecSubf(totmat[2], vec, vec2);
1890                                                         Normalize(totmat[2]);
1891
1892                                                         /* the y axis is fixed */
1893                                                         totmat[1][0] = ownermat[1][0];
1894                                                         totmat[1][1] = ownermat[1][1];
1895                                                         totmat[1][2] = ownermat[1][2];
1896                                                         Normalize(totmat[1]);
1897                                                         
1898                                                         /* the z axis gets mapped onto a third orthogonal vector */
1899                                                         Crossf(totmat[0], totmat[1], totmat[2]);
1900                                                 }
1901                                                         break;
1902                                                 case TRACK_nX: /* LOCK Y TRACK -X */
1903                                                 {
1904                                                         /* Projection of Vector on the plane */
1905                                                         Projf(vec2, vec, ownermat[1]);
1906                                                         VecSubf(totmat[0], vec, vec2);
1907                                                         Normalize(totmat[0]);
1908                                                         VecMulf(totmat[0],-1);
1909
1910                                                         /* the y axis is fixed */
1911                                                         totmat[1][0] = ownermat[1][0];
1912                                                         totmat[1][1] = ownermat[1][1];
1913                                                         totmat[1][2] = ownermat[1][2];
1914                                                         Normalize(totmat[1]);
1915                                                         
1916                                                         /* the z axis gets mapped onto a third orthogonal vector */
1917                                                         Crossf(totmat[2], totmat[0], totmat[1]);
1918                                                 }
1919                                                         break;
1920                                                 case TRACK_nZ: /* LOCK Y TRACK -Z */
1921                                                 {
1922                                                         /* Projection of Vector on the plane */
1923                                                         Projf(vec2, vec, ownermat[1]);
1924                                                         VecSubf(totmat[2], vec, vec2);
1925                                                         Normalize(totmat[2]);
1926                                                         VecMulf(totmat[2],-1);
1927
1928                                                         /* the y axis is fixed */
1929                                                         totmat[1][0] = ownermat[1][0];
1930                                                         totmat[1][1] = ownermat[1][1];
1931                                                         totmat[1][2] = ownermat[1][2];
1932                                                         Normalize(totmat[1]);
1933                                                         
1934                                                         /* the z axis gets mapped onto a third orthogonal vector */
1935                                                         Crossf(totmat[0], totmat[1], totmat[2]);
1936                                                 }
1937                                                         break;
1938                                                 default:
1939                                                 {
1940                                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1941                                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1942                                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1943                                                 }
1944                                                         break;
1945                                         }
1946                                 }
1947                                         break;
1948                                 case LOCK_Z: /* LOCK Z */
1949                                 {
1950                                         switch (data->trackflag) {
1951                                                 case TRACK_X: /* LOCK Z TRACK X */
1952                                                 {
1953                                                         /* Projection of Vector on the plane */
1954                                                         Projf(vec2, vec, ownermat[2]);
1955                                                         VecSubf(totmat[0], vec, vec2);
1956                                                         Normalize(totmat[0]);
1957                                                         
1958                                                         /* the z axis is fixed */
1959                                                         totmat[2][0] = ownermat[2][0];
1960                                                         totmat[2][1] = ownermat[2][1];
1961                                                         totmat[2][2] = ownermat[2][2];
1962                                                         Normalize(totmat[2]);
1963                                                         
1964                                                         /* the x axis gets mapped onto a third orthogonal vector */
1965                                                         Crossf(totmat[1], totmat[2], totmat[0]);
1966                                                 }
1967                                                         break;
1968                                                 case TRACK_Y: /* LOCK Z TRACK Y */
1969                                                 {
1970                                                         /* Projection of Vector on the plane */
1971                                                         Projf(vec2, vec, ownermat[2]);
1972                                                         VecSubf(totmat[1], vec, vec2);
1973                                                         Normalize(totmat[1]);
1974                                                         
1975                                                         /* the z axis is fixed */
1976                                                         totmat[2][0] = ownermat[2][0];
1977                                                         totmat[2][1] = ownermat[2][1];
1978                                                         totmat[2][2] = ownermat[2][2];
1979                                                         Normalize(totmat[2]);
1980                                                                 
1981                                                         /* the x axis gets mapped onto a third orthogonal vector */
1982                                                         Crossf(totmat[0], totmat[1], totmat[2]);
1983                                                 }
1984                                                         break;
1985                                                 case TRACK_nX: /* LOCK Z TRACK -X */
1986                                                 {
1987                                                         /* Projection of Vector on the plane */
1988                                                         Projf(vec2, vec, ownermat[2]);
1989                                                         VecSubf(totmat[0], vec, vec2);
1990                                                         Normalize(totmat[0]);
1991                                                         VecMulf(totmat[0],-1);
1992                                                         
1993                                                         /* the z axis is fixed */
1994                                                         totmat[2][0] = ownermat[2][0];
1995                                                         totmat[2][1] = ownermat[2][1];
1996                                                         totmat[2][2] = ownermat[2][2];
1997                                                         Normalize(totmat[2]);
1998                                                         
1999                                                         /* the x axis gets mapped onto a third orthogonal vector */
2000                                                         Crossf(totmat[1], totmat[2], totmat[0]);
2001                                                 }
2002                                                         break;
2003                                                 case TRACK_nY: /* LOCK Z TRACK -Y */
2004                                                 {
2005                                                         /* Projection of Vector on the plane */
2006                                                         Projf(vec2, vec, ownermat[2]);
2007                                                         VecSubf(totmat[1], vec, vec2);
2008                                                         Normalize(totmat[1]);
2009                                                         VecMulf(totmat[1],-1);
2010                                                         
2011                                                         /* the z axis is fixed */
2012                                                         totmat[2][0] = ownermat[2][0];
2013                                                         totmat[2][1] = ownermat[2][1];
2014                                                         totmat[2][2] = ownermat[2][2];
2015                                                         Normalize(totmat[2]);
2016                                                                 
2017                                                         /* the x axis gets mapped onto a third orthogonal vector */
2018                                                         Crossf(totmat[0], totmat[1], totmat[2]);
2019                                                 }
2020                                                         break;
2021                                                 default:
2022                                                 {
2023                                                                 totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
2024                                                                 totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
2025                                                                 totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
2026                                                 }
2027                                                         break;
2028                                         }
2029                                 }
2030                                         break;
2031                                 default:
2032                                         {
2033                                                 totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
2034                                                 totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
2035                                                 totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
2036                                         }
2037                                         break;
2038                                 }
2039                                 /* Block to keep matrix heading */
2040                                 tmpmat[0][0] = ownermat[0][0];tmpmat[0][1] = ownermat[0][1];tmpmat[0][2] = ownermat[0][2];
2041                                 tmpmat[1][0] = ownermat[1][0];tmpmat[1][1] = ownermat[1][1];tmpmat[1][2] = ownermat[1][2];
2042                                 tmpmat[2][0] = ownermat[2][0];tmpmat[2][1] = ownermat[2][1];tmpmat[2][2] = ownermat[2][2];
2043                                 Normalize(tmpmat[0]);
2044                                 Normalize(tmpmat[1]);
2045                                 Normalize(tmpmat[2]);
2046                                 Mat3Inv(invmat,tmpmat);
2047                                 Mat3MulMat3(tmpmat, totmat, invmat);
2048                                 totmat[0][0] = tmpmat[0][0];totmat[0][1] = tmpmat[0][1];totmat[0][2] = tmpmat[0][2];
2049                                 totmat[1][0] = tmpmat[1][0];totmat[1][1] = tmpmat[1][1];totmat[1][2] = tmpmat[1][2];
2050                                 totmat[2][0] = tmpmat[2][0];totmat[2][1] = tmpmat[2][1];totmat[2][2] = tmpmat[2][2];
2051
2052                                 Mat4CpyMat4(tmat, ownermat);
2053
2054                                 mdet = Det3x3(  totmat[0][0],totmat[0][1],totmat[0][2],
2055                                                                 totmat[1][0],totmat[1][1],totmat[1][2],
2056                                                                 totmat[2][0],totmat[2][1],totmat[2][2]);
2057                                 if (mdet==0)
2058                                 {
2059                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
2060                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
2061                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
2062                                 }
2063
2064                                 /* apply out transformaton to the object */
2065                                 Mat4MulMat34(ownermat, totmat, tmat);
2066                         }
2067                 }
2068                 break;
2069         case CONSTRAINT_TYPE_FOLLOWPATH:
2070                 {
2071                         bFollowPathConstraint *data;
2072                         float obmat[4][4];
2073                         float size[3], obsize[3];
2074                         
2075                         data = constraint->data;                        
2076                         
2077                         if (data->tar) {
2078                                 /* get Object local transform (loc/rot/size) to determine transformation from path */
2079                                 //object_to_mat4(ob, obmat);
2080                                 Mat4CpyMat4(obmat, ownermat); // FIXME!!!
2081                                 
2082                                 /* get scaling of object before applying constraint */
2083                                 Mat4ToSize(ownermat, size);
2084                                 
2085                                 /* apply targetmat - containing location on path, and rotation */
2086                                 Mat4MulSerie(ownermat, targetmat, obmat, NULL, NULL, NULL, NULL, NULL, NULL);
2087                                 
2088                                 /* un-apply scaling caused by path */
2089                                 Mat4ToSize(ownermat, obsize);
2090                                 if (obsize[0] != 0)
2091                                         VecMulf(ownermat[0], size[0] / obsize[0]);
2092                                 if (obsize[1] != 0)
2093                                         VecMulf(ownermat[1], size[1] / obsize[1]);
2094                                 if (obsize[2] != 0)
2095                                         VecMulf(ownermat[2], size[2] / obsize[2]);
2096                         }
2097                 }
2098                 break;
2099         case CONSTRAINT_TYPE_STRETCHTO:
2100         {
2101             bStretchToConstraint *data;
2102             float size[3],scale[3],vec[3],xx[3],zz[3],orth[3];
2103             float totmat[3][3];
2104             float tmat[4][4];
2105             float dist;
2106                         
2107             data = constraint->data;            
2108             Mat4ToSize (ownermat, size);
2109             
2110             if (data->tar) {
2111                 /* store X orientation before destroying obmat */
2112                 xx[0] = ownermat[0][0];
2113                 xx[1] = ownermat[0][1];
2114                 xx[2] = ownermat[0][2];
2115                 Normalize(xx);
2116                                 
2117                 /* store Z orientation before destroying obmat */
2118                 zz[0] = ownermat[2][0];
2119                 zz[1] = ownermat[2][1];
2120                 zz[2] = ownermat[2][2];
2121                 Normalize(zz);
2122                                 
2123                                 VecSubf(vec, ownermat[3], targetmat[3]);
2124                                 vec[0] /= size[0];
2125                                 vec[1] /= size[1];
2126                                 vec[2] /= size[2];
2127
2128                                 dist = Normalize(vec);
2129                 //dist = VecLenf( ob->obmat[3], targetmat[3]);
2130
2131                 if (data->orglength == 0)  data->orglength = dist;
2132                 if (data->bulge ==0) data->bulge = 1.0;
2133
2134                 scale[1] = dist/data->orglength;
2135                 switch (data->volmode) {
2136                 /* volume preserving scaling */
2137                 case VOLUME_XZ :
2138                     scale[0] = 1.0f - (float)sqrt(data->bulge) + (float)sqrt(data->bulge*(data->orglength/dist));
2139                     scale[2] = scale[0];
2140                     break;
2141                 case VOLUME_X:
2142                     scale[0] = 1.0f + data->bulge * (data->orglength /dist - 1);
2143                     scale[2] = 1.0;
2144                     break;
2145                 case VOLUME_Z:
2146                     scale[0] = 1.0;
2147                     scale[2] = 1.0f + data->bulge * (data->orglength /dist - 1);
2148                     break;
2149                     /* don't care for volume */
2150                 case NO_VOLUME:
2151                     scale[0] = 1.0;
2152                     scale[2] = 1.0;
2153                     break;
2154                 default: /* should not happen, but in case*/
2155                     return;    
2156                 } /* switch (data->volmode) */
2157
2158                 /* Clear the object's rotation and scale */
2159                                 ownermat[0][0]=size[0]*scale[0];
2160                                 ownermat[0][1]=0;
2161                                 ownermat[0][2]=0;
2162                                 ownermat[1][0]=0;
2163                                 ownermat[1][1]=size[1]*scale[1];
2164                                 ownermat[1][2]=0;
2165                                 ownermat[2][0]=0;
2166                                 ownermat[2][1]=0;
2167                                 ownermat[2][2]=size[2]*scale[2];
2168                 
2169                                 VecSubf(vec, ownermat[3], targetmat[3]);
2170                                 Normalize(vec);
2171                                 
2172                 /* new Y aligns  object target connection*/
2173                 totmat[1][0] = -vec[0];
2174                 totmat[1][1] = -vec[1];
2175                 totmat[1][2] = -vec[2];
2176                 switch (data->plane) {
2177                 case PLANE_X:
2178                     /* build new Z vector */
2179                     /* othogonal to "new Y" "old X! plane */
2180                     Crossf(orth, vec, xx);
2181                     Normalize(orth);
2182                     
2183                     /* new Z*/
2184                     totmat[2][0] = orth[0];
2185                     totmat[2][1] = orth[1];
2186                     totmat[2][2] = orth[2];
2187                     
2188                     /* we decided to keep X plane*/
2189                     Crossf(xx, orth, vec);
2190                     Normalize(xx);
2191                     totmat[0][0] = xx[0];
2192                     totmat[0][1] = xx[1];
2193                     totmat[0][2] = xx[2];
2194                     break;
2195                 case PLANE_Z:
2196                     /* build new X vector */
2197                     /* othogonal to "new Y" "old Z! plane */
2198                     Crossf(orth, vec, zz);
2199                     Normalize(orth);
2200                     
2201                     /* new X */
2202                     totmat[0][0] = -orth[0];
2203                     totmat[0][1] = -orth[1];
2204                     totmat[0][2] = -orth[2];
2205                     
2206                     /* we decided to keep Z */
2207                     Crossf(zz, orth, vec);
2208                     Normalize(zz);
2209                     totmat[2][0] = zz[0];
2210                     totmat[2][1] = zz[1];
2211                     totmat[2][2] = zz[2];
2212                     break;
2213                 } /* switch (data->plane) */
2214                 
2215                 Mat4CpyMat4(tmat, ownermat);
2216                                 
2217                 Mat4MulMat34(ownermat, totmat, tmat);
2218             }
2219         }
2220         break;
2221         case CONSTRAINT_TYPE_LOCLIMIT:
2222                 {
2223                         bLocLimitConstraint *data;
2224
2225                         data = constraint->data;
2226                         
2227                         if (data->flag & LIMIT_XMIN) {
2228                                 if(ownermat[3][0] < data->xmin)
2229                                         ownermat[3][0] = data->xmin;
2230                         }
2231                         if (data->flag & LIMIT_XMAX) {
2232                                 if (ownermat[3][0] > data->xmax)
2233                                         ownermat[3][0] = data->xmax;
2234                         }
2235                         if (data->flag & LIMIT_YMIN) {
2236                                 if(ownermat[3][1] < data->ymin)
2237                                         ownermat[3][1] = data->ymin;
2238                         }
2239                         if (data->flag & LIMIT_YMAX) {
2240                                 if (ownermat[3][1] > data->ymax)
2241                                         ownermat[3][1] = data->ymax;
2242                         }
2243                         if (data->flag & LIMIT_ZMIN) {
2244                                 if(ownermat[3][2] < data->zmin) 
2245                                         ownermat[3][2] = data->zmin;
2246                         }
2247                         if (data->flag & LIMIT_ZMAX) {
2248                                 if (ownermat[3][2] > data->zmax)
2249                                         ownermat[3][2] = data->zmax;
2250                         }
2251                 }
2252                 break;
2253         case CONSTRAINT_TYPE_ROTLIMIT:
2254                 {
2255                         bRotLimitConstraint *data;
2256                         float loc[3];
2257                         float eul[3];
2258                         float size[3];
2259                         
2260                         data = constraint->data;
2261                         
2262                         VECCOPY(loc, ownermat[3]);
2263                         Mat4ToSize(ownermat, size);
2264                         
2265                         Mat4ToEul(ownermat, eul);
2266                         
2267                         /* eulers: radians to degrees! */
2268                         eul[0] = (eul[0] / M_PI * 180);
2269                         eul[1] = (eul[1] / M_PI * 180);
2270                         eul[2] = (eul[2] / M_PI * 180);
2271                         
2272                         /* limiting of euler values... */
2273                         if (data->flag & LIMIT_XROT) {
2274                                 if (eul[0] < data->xmin) 
2275                                         eul[0] = data->xmin;
2276                                         
2277                                 if (eul[0] > data->xmax)
2278                                         eul[0] = data->xmax;
2279                         }
2280                         if (data->flag & LIMIT_YROT) {
2281                                 if (eul[1] < data->ymin)
2282                                         eul[1] = data->ymin;
2283                                         
2284                                 if (eul[1] > data->ymax)
2285                                         eul[1] = data->ymax;
2286                         }
2287                         if (data->flag & LIMIT_ZROT) {
2288                                 if (eul[2] < data->zmin)
2289                                         eul[2] = data->zmin;
2290                                         
2291                                 if (eul[2] > data->zmax)
2292                                         eul[2] = data->zmax;
2293                         }
2294                                 
2295                         /* eulers: degrees to radians ! */
2296                         eul[0] = (eul[0] / 180 * M_PI); 
2297                         eul[1] = (eul[1] / 180 * M_PI);
2298                         eul[2] = (eul[2] / 180 * M_PI);
2299                         
2300                         LocEulSizeToMat4(ownermat, loc, eul, size);
2301                 }
2302                 break;
2303         case CONSTRAINT_TYPE_SIZELIMIT:
2304                 {
2305                         bSizeLimitConstraint *data;
2306                         float obsize[3], size[3];
2307                         
2308                         data = constraint->data;
2309                         
2310                         Mat4ToSize(ownermat, size);
2311                         Mat4ToSize(ownermat, obsize);
2312                         
2313                         if (data->flag & LIMIT_XMIN) {
2314                                 if (size[0] < data->xmin) 
2315                                         size[0] = data->xmin;   
2316                         }
2317                         if (data->flag & LIMIT_XMAX) {
2318                                 if (size[0] > data->xmax) 
2319                                         size[0] = data->xmax;
2320                         }
2321                         if (data->flag & LIMIT_YMIN) {
2322                                 if (size[1] < data->ymin) 
2323                                         size[1] = data->ymin;   
2324                         }
2325                         if (data->flag & LIMIT_YMAX) {
2326                                 if (size[1] > data->ymax) 
2327                                         size[1] = data->ymax;
2328                         }
2329                         if (data->flag & LIMIT_ZMIN) {
2330                                 if (size[2] < data->zmin) 
2331                                         size[2] = data->zmin;   
2332                         }
2333                         if (data->flag & LIMIT_ZMAX) {
2334                                 if (size[2] > data->zmax) 
2335                                         size[2] = data->zmax;
2336                         }
2337                         
2338                         VecMulf(ownermat[0], size[0]/obsize[0]);
2339                         VecMulf(ownermat[1], size[1]/obsize[1]);
2340                         VecMulf(ownermat[2], size[2]/obsize[2]);
2341                 }
2342                 break;
2343         case CONSTRAINT_TYPE_RIGIDBODYJOINT:
2344         {
2345                         /* Do nothing. The GameEngine will take care of this.*/
2346         }
2347         break;  
2348         case CONSTRAINT_TYPE_CLAMPTO:
2349                 {
2350                         bClampToConstraint *data;
2351                         Curve *cu;
2352                         float obmat[4][4], targetMatrix[4][4], ownLoc[3];
2353                         float curveMin[3], curveMax[3];
2354                         
2355                         data = constraint->data;
2356                         
2357                         /* prevent crash if user deletes curve */
2358                         if ((data->tar == NULL) || (data->tar->type != OB_CURVE) ) 
2359                                 return;
2360                         else
2361                                 cu= data->tar->data;
2362                         
2363                         Mat4CpyMat4(obmat, ownermat);
2364                         Mat4One(targetMatrix);
2365                         VECCOPY(ownLoc, obmat[3]);
2366                         
2367                         INIT_MINMAX(curveMin, curveMax)
2368                         minmax_object(data->tar, curveMin, curveMax);
2369                         
2370                         /* get targetmatrix */
2371                         if (cu->path && cu->path->data) {
2372                                 float vec[4], dir[3], totmat[4][4];
2373                                 float curvetime;
2374                                 short clamp_axis;
2375                                 
2376                                 /* find best position on curve */
2377                                 /* 1. determine which axis to sample on? */
2378                                 if (data->flag==CLAMPTO_AUTO) {
2379                                         float size[3];
2380                                         VecSubf(size, curveMax, curveMin);
2381                                         
2382                                         /* find axis along which the bounding box has the greatest
2383                                          * extent. Otherwise, default to the x-axis, as that is quite
2384                                          * frequently used.
2385                                          */
2386                                         if ((size[2]>size[0]) && (size[2]>size[1]))
2387                                                 clamp_axis= CLAMPTO_Z;
2388                                         else if ((size[1]>size[0]) && (size[1]>size[2]))
2389                                                 clamp_axis= CLAMPTO_Y;
2390                                         else
2391                                                 clamp_axis = CLAMPTO_X;
2392                                 }
2393                                 else 
2394                                         clamp_axis= data->flag;
2395                                         
2396                                 /* 2. determine position relative to curve on a 0-1 scale */
2397                                 if (clamp_axis > 0) clamp_axis--;
2398                                 if (ownLoc[clamp_axis] <= curveMin[clamp_axis])
2399                                         curvetime = 0.0;
2400                                 else if (ownLoc[clamp_axis] >= curveMax[clamp_axis])
2401                                         curvetime = 1.0;
2402                                 else
2403                                         curvetime = (ownLoc[clamp_axis] - curveMin[clamp_axis]) / (curveMax[clamp_axis] - curveMin[clamp_axis]);
2404                                 
2405                                 /* 3. position on curve */
2406                                 if(where_on_path(data->tar, curvetime, vec, dir) ) {
2407                                         Mat4One(totmat);
2408                                         VECCOPY(totmat[3], vec);
2409                                         
2410                                         Mat4MulSerie(targetMatrix, data->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
2411                                 }
2412                         }
2413                         
2414                         /* obtain final object position */
2415                         VECCOPY(ownermat[3], targetMatrix[3]);
2416                 }
2417                 break;
2418         case CONSTRAINT_TYPE_CHILDOF: 
2419                 {
2420                         bChildOfConstraint *data;
2421                         
2422                         data = constraint->data;
2423                         
2424                         /* only evaluate if there is a target */
2425                         if (data->tar) {
2426                                 float parmat[4][4], invmat[4][4], tempmat[4][4];
2427                                 float loc[3], eul[3], size[3];
2428                                 float loco[3], eulo[3], sizo[3];
2429                                 
2430                                 /* get offset (parent-inverse) matrix */
2431                                 Mat4CpyMat4(invmat, data->invmat);
2432                                 
2433                                 /* extract components of both matrices */
2434                                 VECCOPY(loc, targetmat[3]);
2435                                 Mat4ToEul(targetmat, eul);
2436                                 Mat4ToSize(targetmat, size);
2437                                 
2438                                 VECCOPY(loco, invmat[3]);
2439                                 Mat4ToEul(invmat, eulo);
2440                                 Mat4ToSize(invmat, sizo);
2441                                 
2442                                 /* disable channels not enabled */
2443                                 if (!(data->flag & CHILDOF_LOCX)) loc[0]= loco[0]= 0.0f;
2444                                 if (!(data->flag & CHILDOF_LOCY)) loc[1]= loco[1]= 0.0f;
2445                                 if (!(data->flag & CHILDOF_LOCZ)) loc[2]= loco[2]= 0.0f;
2446                                 if (!(data->flag & CHILDOF_ROTX)) eul[0]= eulo[0]= 0.0f;
2447                                 if (!(data->flag & CHILDOF_ROTY)) eul[1]= eulo[1]= 0.0f;
2448                                 if (!(data->flag & CHILDOF_ROTZ)) eul[2]= eulo[2]= 0.0f;
2449                                 if (!(data->flag & CHILDOF_SIZEX)) size[0]= sizo[0]= 1.0f;
2450                                 if (!(data->flag & CHILDOF_SIZEY)) size[1]= sizo[1]= 1.0f;
2451                                 if (!(data->flag & CHILDOF_SIZEZ)) size[2]= sizo[2]= 1.0f;
2452                                 
2453                                 /* make new target mat and offset mat */
2454                                 LocEulSizeToMat4(targetmat, loc, eul, size);
2455                                 LocEulSizeToMat4(invmat, loco, eulo, sizo);
2456                                 
2457                                 /* multiply target (parent matrix) by offset (parent inverse) to get 
2458                                  * the effect of the parent that will be exherted on the owner
2459                                  */
2460                                 Mat4MulMat4(parmat, invmat, targetmat);
2461                                 
2462                                 /* now multiply the parent matrix by the owner matrix to get the 
2463                                  * the effect of this constraint (i.e.  owner is 'parented' to parent)
2464                                  */
2465                                 Mat4CpyMat4(tempmat, ownermat);
2466                                 Mat4MulMat4(ownermat, tempmat, parmat); 
2467                         }
2468                 }
2469                 break;
2470         default:
2471                 printf ("Error: Unknown constraint type\n");
2472                 break;
2473         }
2474 }
2475
2476 /* this function is called whenever constraints need to be evaluated  */
2477 void solve_constraints (ListBase *conlist, bConstraintOb *cob, float ctime)
2478 {
2479         bConstraint *con;
2480         void *ownerdata;
2481         float tarmat[4][4], oldmat[4][4];
2482         float solution[4][4], delta[4][4], imat[4][4];
2483         float enf;
2484
2485         /* check that there is a valid constraint object to evaluate */
2486         if (cob == NULL)
2487                 return;
2488         
2489         /* loop over available constraints, solving and blending them */
2490         for (con= conlist->first; con; con= con->next) {
2491                 /* this we can skip completely */
2492                 if (con->flag & CONSTRAINT_DISABLE) continue;
2493                 /* and inverse kinematics is solved seperate */ 
2494                 if (con->type==CONSTRAINT_TYPE_KINEMATIC) continue;
2495                 /* rigidbody is really a game-engine thing - and is not solved here */
2496                 if (con->type==CONSTRAINT_TYPE_RIGIDBODYJOINT) continue;
2497                 
2498                 /* influence of constraint */
2499                 /* value should have been set from IPO's/Constraint Channels already */
2500                 enf = con->enforce;
2501                 
2502                 /* move target/owner into right spaces */
2503                 constraint_mat_convertspace(cob->ob, cob->pchan, cob->matrix, CONSTRAINT_SPACE_WORLD, con->ownspace);
2504                 
2505                 /* Get the target matrix */
2506                 ownerdata= ((cob->pchan)? (void *)cob->pchan : (void *)cob->ob);
2507                 get_constraint_target_matrix(con, cob->type, ownerdata, tarmat, ctime);
2508                 
2509                 Mat4CpyMat4(oldmat, cob->matrix);
2510                 
2511                 /* solve the constraint */
2512                 evaluate_constraint(con, cob->matrix, tarmat);
2513                 
2514                 /* Interpolate the enforcement, to blend result of constraint into final owner transform */
2515                 /* 1. Remove effects of original matrix from constraint solution ==> delta */
2516                 Mat4Invert(imat, oldmat);
2517                 Mat4CpyMat4(solution, cob->matrix);
2518                 Mat4MulMat4(delta, solution, imat);
2519                 
2520                 /* 2. If constraint influence is not full strength, then interpolate
2521                  *      identity_matrix --> delta_matrix to get the effect the constraint actually exerts
2522                  */
2523                 if (enf < 1.0) {
2524                         float identity[4][4];
2525                         Mat4One(identity);
2526                         Mat4BlendMat4(delta, identity, delta, enf);
2527                 }
2528                 
2529                 /* 3. Now multiply the delta by the matrix in use before the evaluation */
2530                 Mat4MulMat4(cob->matrix, delta, oldmat);
2531                 
2532                 /* move target/owner back into worldspace for next constraint/other business */
2533                 if ((con->flag & CONSTRAINT_SPACEONCE) == 0) 
2534                         constraint_mat_convertspace(cob->ob, cob->pchan, cob->matrix, con->ownspace, CONSTRAINT_SPACE_WORLD);
2535         }
2536 }