Final merge of HEAD (bf-blender) into the orange branch.
[blender.git] / source / blender / blenkernel / intern / constraint.c
1 /**
2  * $Id$
3  *
4  * ***** BEGIN GPL/BL DUAL LICENSE BLOCK *****
5  *
6  * This program is free software; you can redistribute it and/or
7  * modify it under the terms of the GNU General Public License
8  * as published by the Free Software Foundation; either version 2
9  * of the License, or (at your option) any later version. The Blender
10  * Foundation also sells licenses for use in proprietary software under
11  * the Blender License.  See http://www.blender.org/BL/ for information
12  * about this.
13  *
14  * This program is distributed in the hope that it will be useful,
15  * but WITHOUT ANY WARRANTY; without even the implied warranty of
16  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
17  * GNU General Public License for more details.
18  *
19  * You should have received a copy of the GNU General Public License
20  * along with this program; if not, write to the Free Software Foundation,
21  * Inc., 59 Temple Place - Suite 330, Boston, MA  02111-1307, USA.
22  *
23  * The Original Code is Copyright (C) 2001-2002 by NaN Holding BV.
24  * All rights reserved.
25  *
26  * The Original Code is: all of this file.
27  *
28  * Contributor(s): none yet.
29  *
30  * ***** END GPL/BL DUAL LICENSE BLOCK *****
31  */
32
33 #include <stdio.h> 
34 #include <string.h>
35 #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
62 #include "blendef.h"
63
64 #ifdef HAVE_CONFIG_H
65 #include <config.h>
66 #endif
67
68 #ifndef M_PI
69 #define M_PI            3.14159265358979323846
70 #endif
71
72 /* used by object.c */
73 void Mat4BlendMat4(float [][4], float [][4], float [][4], float );
74
75 /* Local function prototypes */
76
77 /* ********************* Data level ****************** */
78
79 void free_constraint_data (bConstraint *con)
80 {
81         if (con->data){
82                 switch (con->type){
83                         default:
84                                 break;
85                 };
86                 
87                 MEM_freeN (con->data);
88         }
89 }
90
91 void free_constraints (ListBase *conlist)
92 {
93         bConstraint *con;
94         
95         /* Do any specific freeing */
96         for (con=conlist->first; con; con=con->next) {
97                 free_constraint_data (con);
98         }
99         
100         /* Free the whole list */
101         BLI_freelistN(conlist);
102 }
103
104 void free_constraint_channels (ListBase *chanbase)
105 {
106         bConstraintChannel *chan;
107         
108         for (chan=chanbase->first; chan; chan=chan->next)
109         {
110                 if (chan->ipo){
111                         chan->ipo->id.us--;
112                 }
113         }
114         
115         BLI_freelistN(chanbase);
116 }
117
118 void relink_constraints (struct ListBase *list)
119 {
120         bConstraint *con;
121         
122         for (con = list->first; con; con=con->next){
123                 switch (con->type){
124                         case CONSTRAINT_TYPE_KINEMATIC:
125                         {
126                                 bKinematicConstraint *data;
127                                 data = con->data;
128                                 
129                                 ID_NEW(data->tar);
130                         }
131                                 break;
132                         case CONSTRAINT_TYPE_NULL:
133                         {
134                         }
135                                 break;
136                         case CONSTRAINT_TYPE_TRACKTO:
137                         {
138                                 bTrackToConstraint *data;
139                                 data = con->data;
140                                 
141                                 ID_NEW(data->tar);
142                         }
143                                 break;
144                         case CONSTRAINT_TYPE_MINMAX:
145                         {
146                                 bMinMaxConstraint *data;
147                                 data = con->data;
148                                 
149                                 ID_NEW(data->tar);
150                         }
151                                 break;
152                         case CONSTRAINT_TYPE_LOCKTRACK:
153                         {
154                                 bLockTrackConstraint *data;
155                                 data = con->data;
156                                 
157                                 ID_NEW(data->tar);
158                         }
159                                 break;
160                         case CONSTRAINT_TYPE_ACTION:
161                         {
162                                 bActionConstraint *data;
163                                 data = con->data;
164                                 
165                                 ID_NEW(data->tar);
166                         }
167                                 break;
168                         case CONSTRAINT_TYPE_LOCLIKE:
169                         {
170                                 bLocateLikeConstraint *data;
171                                 data = con->data;
172                                 
173                                 ID_NEW(data->tar);
174                         }
175                                 break;
176                         case CONSTRAINT_TYPE_ROTLIKE:
177                         {
178                                 bRotateLikeConstraint *data;
179                                 data = con->data;
180                                 
181                                 ID_NEW(data->tar);
182                         }
183                                 break;
184                         case CONSTRAINT_TYPE_FOLLOWPATH:
185                         {
186                                 bFollowPathConstraint *data;
187                                 data = con->data;
188                                 
189                                 ID_NEW(data->tar);
190                         }
191                                 break;
192                         case CONSTRAINT_TYPE_STRETCHTO:
193                         {
194                                 bStretchToConstraint *data;
195                                 data = con->data;
196                                 
197                                 ID_NEW(data->tar);
198                         }
199                                 break;
200                                 
201                 }
202         }
203 }
204
205 void copy_constraint_channels (ListBase *dst, ListBase *src)
206 {
207         bConstraintChannel *dchan, *schan;
208         
209         dst->first=dst->last=NULL;
210         duplicatelist(dst, src);
211         
212         for (dchan=dst->first, schan=src->first; dchan; dchan=dchan->next, schan=schan->next){
213                 dchan->ipo = copy_ipo(schan->ipo);
214         }
215 }
216
217 void clone_constraint_channels (ListBase *dst, ListBase *src)
218 {
219         bConstraintChannel *dchan, *schan;
220         
221         dst->first=dst->last=NULL;
222         duplicatelist(dst, src);
223         
224         for (dchan=dst->first, schan=src->first; dchan; dchan=dchan->next, schan=schan->next){
225                 id_us_plus((ID *)dchan->ipo);
226         }
227 }
228
229 void copy_constraints (ListBase *dst, ListBase *src)
230 {
231         bConstraint *con;
232         
233         dst->first= dst->last= NULL;
234         
235         duplicatelist (dst, src);
236         
237         for (con = dst->first; con; con=con->next) {
238                 con->data = MEM_dupallocN (con->data);
239                 /* removed a whole lot of useless code here (ton) */
240         }
241 }
242
243
244 /* **************** Editor Functions **************** */
245
246 char constraint_has_target (bConstraint *con) 
247 {
248         switch (con->type){
249         case CONSTRAINT_TYPE_TRACKTO:
250                 {
251                         bTrackToConstraint *data = con->data;
252                         if (data->tar)
253                                 return 1;
254                 }
255                 break;
256         case CONSTRAINT_TYPE_KINEMATIC:
257                 {
258                         bKinematicConstraint *data = con->data;
259                         if (data->tar)
260                                 return 1;
261                 }
262                 break;
263         case CONSTRAINT_TYPE_FOLLOWPATH:
264                 {
265                         bFollowPathConstraint *data = con->data;
266                         if (data->tar)
267                                 return 1;
268                 }
269                 break;
270         case CONSTRAINT_TYPE_ROTLIKE:
271                 {
272                         bRotateLikeConstraint *data = con->data;
273                         if (data->tar)
274                                 return 1;
275                 }
276                 break;
277         case CONSTRAINT_TYPE_LOCLIKE:
278                 {
279                         bLocateLikeConstraint *data = con->data;
280                         if (data->tar)
281                                 return 1;
282                 }
283                 break;
284         case CONSTRAINT_TYPE_MINMAX:
285                 {
286                         bMinMaxConstraint *data = con->data;
287                         if (data->tar)
288                                 return 1;
289                 }
290                 break;
291         case CONSTRAINT_TYPE_ACTION:
292                 {
293                         bActionConstraint *data = con->data;
294                         if (data->tar)
295                                 return 1;
296                 }
297                 break;
298         case CONSTRAINT_TYPE_LOCKTRACK:
299                 {
300                         bLockTrackConstraint *data = con->data;
301                         if (data->tar)
302                                 return 1;
303                 }
304         case CONSTRAINT_TYPE_STRETCHTO:
305                 {
306                         bStretchToConstraint *data = con->data;
307                         if (data->tar)
308                                 return 1;
309                 }
310                 break;
311         }
312         // Unknown types or CONSTRAINT_TYPE_NULL or no target
313         return 0;
314 }
315
316 Object *get_constraint_target(bConstraint *con, char **subtarget)
317 {
318 /*
319 * If the target for this constraint is target, return a pointer 
320 * to the name for this constraints subtarget ... NULL otherwise
321         */
322         switch (con->type) {
323         case CONSTRAINT_TYPE_ACTION:
324                 {
325                         bActionConstraint *data = con->data;
326                         *subtarget= data->subtarget;
327                         return data->tar;
328                 }
329                 break;
330         case CONSTRAINT_TYPE_LOCLIKE:
331                 {
332                         bLocateLikeConstraint *data = con->data;
333                         *subtarget= data->subtarget;
334                         return data->tar;
335                 }
336                 break;
337         case CONSTRAINT_TYPE_ROTLIKE:
338                 {
339                         bRotateLikeConstraint *data = con->data;
340                         *subtarget= data->subtarget;
341                         return data->tar;
342                 }
343                 break;
344         case CONSTRAINT_TYPE_KINEMATIC:
345                 {
346                         bKinematicConstraint *data = con->data;
347                         *subtarget= data->subtarget;
348                         return data->tar;
349                 }
350                 break;
351         case CONSTRAINT_TYPE_TRACKTO:
352                 {
353                         bTrackToConstraint *data = con->data;
354                         *subtarget= data->subtarget;
355                         return data->tar;
356                 }
357                 break;
358         case CONSTRAINT_TYPE_MINMAX:
359                 {
360                         bMinMaxConstraint *data = con->data;
361                         *subtarget= data->subtarget;
362                         return data->tar;
363                 }
364                 break;
365         case CONSTRAINT_TYPE_LOCKTRACK:
366                 {
367                         bLockTrackConstraint *data = con->data;
368                         *subtarget= data->subtarget;
369                         return data->tar;
370                 }
371                 break;
372         case CONSTRAINT_TYPE_FOLLOWPATH: 
373                 {
374                         bFollowPathConstraint *data = con->data;
375                         *subtarget= NULL;
376                         return data->tar;
377                 }
378                 break;
379         case CONSTRAINT_TYPE_STRETCHTO:
380                 {
381                         bStretchToConstraint *data = con->data;
382                         *subtarget= data->subtarget;
383                         return (data->tar);
384                 }
385                 break;
386         default:
387                 *subtarget= NULL;
388                 break;
389         }
390         
391         return NULL;  
392 }
393
394 void set_constraint_target(bConstraint *con, Object *ob, char *subtarget)
395 {
396         /*
397          * Set the target for this constraint  
398          */
399         switch (con->type) {
400                 case CONSTRAINT_TYPE_ACTION:
401                 {
402                         bActionConstraint *data = con->data;
403                         data->tar= ob;
404                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
405                 }
406                         break;
407                 case CONSTRAINT_TYPE_LOCLIKE:
408                 {
409                         bLocateLikeConstraint *data = con->data;
410                         data->tar= ob;
411                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
412                 }
413                         break;
414                 case CONSTRAINT_TYPE_ROTLIKE:
415                 {
416                         bRotateLikeConstraint *data = con->data;
417                         data->tar= ob;
418                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
419                 }
420                         break;
421                 case CONSTRAINT_TYPE_KINEMATIC:
422                 {
423                         bKinematicConstraint *data = con->data;
424                         data->tar= ob;
425                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
426                 }
427                         break;
428                 case CONSTRAINT_TYPE_TRACKTO:
429                 {
430                         bTrackToConstraint *data = con->data;
431                         data->tar= ob;
432                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
433                 }
434                         break;
435                 case CONSTRAINT_TYPE_LOCKTRACK:
436                 {
437                         bLockTrackConstraint *data = con->data;
438                         data->tar= ob;
439                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
440                 }
441                         break;
442                 case CONSTRAINT_TYPE_FOLLOWPATH: 
443                 {
444                         bFollowPathConstraint *data = con->data;
445                         data->tar= ob;
446                 }
447                         break;
448                 case CONSTRAINT_TYPE_STRETCHTO:
449                 {
450                         bStretchToConstraint *data = con->data;
451                         data->tar= ob;
452                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
453                 }
454                         break;
455                 case CONSTRAINT_TYPE_MINMAX:
456                 {
457                         bMinMaxConstraint *data = (bMinMaxConstraint*)con->data;
458                         data->tar= ob;
459                         if(subtarget) BLI_strncpy(data->subtarget, subtarget, 32);
460                 }
461                         break;
462         }
463 }
464
465 void unique_constraint_name (bConstraint *con, ListBase *list)
466 {
467         char            tempname[64];
468         int                     number;
469         char            *dot;
470         int exists = 0;
471         bConstraint *curcon;
472         
473         /* See if we even need to do this */
474         for (curcon = list->first; curcon; curcon=curcon->next){
475                 if (curcon!=con){
476                         if (!strcmp(curcon->name, con->name)){
477                                 exists = 1;
478                                 break;
479                         }
480                 }
481         }
482         
483         if (!exists)
484                 return;
485
486         /*      Strip off the suffix */
487         dot=strchr(con->name, '.');
488         if (dot)
489                 *dot=0;
490         
491         for (number = 1; number <=999; number++){
492                 sprintf (tempname, "%s.%03d", con->name, number);
493                 
494                 exists = 0;
495                 for (curcon=list->first; curcon; curcon=curcon->next){
496                         if (con!=curcon){
497                                 if (!strcmp (curcon->name, tempname)){
498                                         exists = 1;
499                                         break;
500                                 }
501                         }
502                 }
503                 if (!exists){
504                         strcpy (con->name, tempname);
505                         return;
506                 }
507         }
508 }
509
510 void *new_constraint_data (short type)
511 {
512         void *result;
513         
514         switch (type){
515         case CONSTRAINT_TYPE_KINEMATIC:
516                 {
517                         bKinematicConstraint *data;
518                         data = MEM_callocN(sizeof(bKinematicConstraint), "kinematicConstraint");
519
520                         data->tolerance = (float)0.001;
521                         data->weight= (float)1.0;
522                         data->iterations = 500;
523                         data->flag= CONSTRAINT_IK_TIP;
524                         
525                         result = data;
526                 }
527                 break;
528         case CONSTRAINT_TYPE_NULL:
529                 {
530                         result = NULL;
531                 }
532                 break;
533         case CONSTRAINT_TYPE_TRACKTO:
534                 {
535                         bTrackToConstraint *data;
536                         data = MEM_callocN(sizeof(bTrackToConstraint), "tracktoConstraint");
537
538
539                         data->reserved1 = TRACK_Y;
540                         data->reserved2 = UP_Z;
541
542                         result = data;
543
544                 }
545                 break;
546         case CONSTRAINT_TYPE_MINMAX:
547                 {
548                         bMinMaxConstraint *data;
549                         data = MEM_callocN(sizeof(bMinMaxConstraint), "minmaxConstraint");
550
551
552                         data->minmaxflag = TRACK_Z;
553                         data->offset = 0.0f;
554                         data->cache[0] = data->cache[1] = data->cache[2] = 0.0f;
555                         data->sticky = 0;
556                         data->stuck = 0;
557
558                         result = data;
559
560                 }
561                 break;
562         case CONSTRAINT_TYPE_ROTLIKE:
563                 {
564                         bRotateLikeConstraint *data;
565                         data = MEM_callocN(sizeof(bRotateLikeConstraint), "rotlikeConstraint");
566                         data->flag = ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z;
567                         result = data;
568                 }
569                 break;
570         case CONSTRAINT_TYPE_LOCLIKE:
571                 {
572                         bLocateLikeConstraint *data;
573                         data = MEM_callocN(sizeof(bLocateLikeConstraint), "loclikeConstraint");
574                         data->flag = LOCLIKE_X|LOCLIKE_Y|LOCLIKE_Z;
575                         result = data;
576                 }
577                 break;
578         case CONSTRAINT_TYPE_ACTION:
579                 {
580                         bActionConstraint *data;
581                         data = MEM_callocN(sizeof(bActionConstraint), "actionConstraint");
582                         data->local= 1;
583                         
584                         result = data;
585                 }
586                 break;
587         case CONSTRAINT_TYPE_LOCKTRACK:
588                 {
589                         bLockTrackConstraint *data;
590                         data = MEM_callocN(sizeof(bLockTrackConstraint), "locktrackConstraint");
591
592                         data->trackflag = TRACK_Y;
593                         data->lockflag = LOCK_Z;
594
595                         result = data;
596                 }
597                 break;
598         case CONSTRAINT_TYPE_FOLLOWPATH:
599                 {
600                         bFollowPathConstraint *data;
601                         data = MEM_callocN(sizeof(bFollowPathConstraint), "followpathConstraint");
602
603                         data->trackflag = TRACK_Y;
604                         data->upflag = UP_Z;
605                         data->offset = 0;
606                         data->followflag = 0;
607
608                         result = data;
609                 }
610                 break;
611         case CONSTRAINT_TYPE_STRETCHTO:
612                 {
613                         bStretchToConstraint *data;
614                         data = MEM_callocN(sizeof(bStretchToConstraint), "StretchToConstraint");
615
616                         data->volmode = 0;
617                         data->plane = 0;
618                         data->orglength = 0.0; 
619                         data->bulge = 1.0;
620                         result = data;
621                 }
622                 break; 
623         default:
624                 result = NULL;
625                 break;
626         }
627
628         return result;
629 }
630
631 bConstraintChannel *get_constraint_channel (ListBase *list, const char *name)
632 {
633         bConstraintChannel *chan;
634
635         for (chan = list->first; chan; chan=chan->next) {
636                 if (!strcmp(name, chan->name)) {
637                         return chan;
638                 }
639         }
640         return NULL;
641 }
642
643 /* finds or creates new constraint channel */
644 bConstraintChannel *verify_constraint_channel (ListBase *list, const char *name)
645 {
646         bConstraintChannel *chan;
647         
648         chan= get_constraint_channel (list, name);
649         if(chan==NULL) {
650                 chan= MEM_callocN(sizeof(bConstraintChannel), "new constraint chan");
651                 BLI_addtail(list, chan);
652                 strcpy(chan->name, name);
653         }
654         
655         return chan;
656 }
657
658
659 /* ***************** Evaluating ********************* */
660
661 /* does ipos only */
662 void do_constraint_channels (ListBase *conbase, ListBase *chanbase, float ctime)
663 {
664         bConstraint *con;
665         bConstraintChannel *chan;
666         IpoCurve *icu=NULL;
667         
668         for (con=conbase->first; con; con=con->next) {
669                 chan = get_constraint_channel(chanbase, con->name);
670                 if (chan && chan->ipo){
671                         calc_ipo(chan->ipo, ctime);
672                         for (icu=chan->ipo->curve.first; icu; icu=icu->next){
673                                 switch (icu->adrcode){
674                                 case CO_ENFORCE:
675                                         con->enforce = icu->curval;
676                                         if (con->enforce<0.0f) con->enforce= 0.0f;
677                                         else if (con->enforce>1.0f) con->enforce= 1.0f;
678                                         break;
679                                 }
680                         }
681                 }
682         }
683 }
684
685 void Mat4BlendMat4(float out[][4], float dst[][4], float src[][4], float srcweight)
686 {
687         float squat[4], dquat[4], fquat[4];
688         float ssize[3], dsize[3], fsize[4];
689         float sloc[3], dloc[3], floc[3];
690         float mat3[3][3], dstweight;
691         float qmat[3][3], smat[3][3];
692         int i;
693
694         dstweight = 1.0F-srcweight;
695
696         Mat3CpyMat4(mat3, dst);
697         Mat3ToQuat(mat3, dquat);
698         Mat3ToSize(mat3, dsize);
699         VECCOPY (dloc, dst[3]);
700
701         Mat3CpyMat4(mat3, src);
702         Mat3ToQuat(mat3, squat);
703         Mat3ToSize(mat3, ssize);
704         VECCOPY (sloc, src[3]);
705         
706         /* Do the actual blend */
707         for (i=0; i<3; i++){
708                 floc[i] = (dloc[i]*dstweight) + (sloc[i]*srcweight);
709                 fsize[i] = 1.0f + ((dsize[i]-1.0f)*dstweight) + ((ssize[i]-1.0f)*srcweight);
710                 fquat[i+1] = (dquat[i+1]*dstweight) + (squat[i+1]*srcweight);
711         }
712         
713         /* Do one more iteration for the quaternions only and normalize the quaternion if needed */
714         fquat[0] = 1.0f + ((dquat[0]-1.0f)*dstweight) + ((squat[0]-1.0f)*srcweight);
715         NormalQuat (fquat);
716
717         QuatToMat3(fquat, qmat);
718         SizeToMat3(fsize, smat);
719
720         Mat3MulMat3(mat3, qmat, smat);
721         Mat4CpyMat3(out, mat3);
722         VECCOPY (out[3], floc);
723 }
724
725 static void constraint_target_to_mat4 (Object *ob, const char *substring, float mat[][4], float size[3], float ctime)
726 {
727
728         /*      Case OBJECT */
729         if (!strlen(substring)) {
730                 Mat4CpyMat4 (mat, ob->obmat);
731                 VECCOPY (size, ob->size);  // whats this for, hack! (ton)
732         }
733         /*      Case BONE */
734         else {
735                 bPoseChannel *pchan;
736                 float   bsize[3]={1, 1, 1};
737
738                 pchan = get_pose_channel(ob->pose, substring);
739                 if (pchan){
740                         /**
741                          *      Multiply the objectspace bonematrix by the skeletons's global
742                          *      transform to obtain the worldspace transformation of the target
743                          */
744                         Mat4MulMat4 (mat, pchan->pose_mat, ob->obmat);
745                 } 
746                 else
747                         Mat4CpyMat4 (mat, ob->obmat);
748
749                 VECCOPY(size, bsize);   // whats this for, hack! (ton)
750         }
751 }
752
753 /* called during solve_constraints */
754 /* also for make_parent, to find correct inverse of "follow path" */
755 /* warning, ownerdata is void... is not Bone anymore, but PoseChannel or Object */
756 short get_constraint_target_matrix (bConstraint *con, short ownertype, void* ownerdata, float mat[][4], float size[3], float ctime)
757 {
758         short valid=0;
759
760         switch (con->type){
761         case CONSTRAINT_TYPE_NULL:
762                 {
763                         Mat4One(mat);
764                 }
765                 break;
766         case CONSTRAINT_TYPE_ACTION:
767                 {
768                         if (ownertype == TARGET_BONE) {
769                                 extern void chan_calc_mat(bPoseChannel *chan);
770                                 bActionConstraint *data = (bActionConstraint*)con->data;
771                                 bPose *pose;
772                                 bPoseChannel *pchan, *tchan;
773                                 float tempmat3[3][3];
774                                 float eul[3];
775                                 float s,t;
776                                 
777                                 Mat4One(mat);   // return mat
778                                 
779                                 if (data->tar==NULL) return 0;
780                                 
781                                 /* need proper check for bone... */
782                                 if(data->subtarget[0]) {
783                                         pchan = get_pose_channel(data->tar->pose, data->subtarget);
784                                         if (pchan) {
785                                                 float arm_mat[3][3], pose_mat[3][3];            /* arm mat should be bone mat! bug... */
786                                                 
787                                                 Mat3CpyMat4(arm_mat, pchan->bone->arm_mat);
788                                                 Mat3CpyMat4(pose_mat, pchan->pose_mat);
789                                                 
790                                                 /* new; true local rotation constraint */
791                                                 if(data->local) {
792                                                         float diff_mat[3][3], par_mat[3][3], ipar_mat[3][3];
793                                                         /* we need the local rotation = current rotation - (parent rotation + restpos) */
794                                                         
795                                                         if (pchan->parent) {
796                                                                 Mat3CpyMat4(par_mat, pchan->parent->pose_mat);
797                                                                 Mat3MulMat3(diff_mat, par_mat, arm_mat);
798                                                                 
799                                                                 Mat3Inv(ipar_mat, diff_mat);
800                                                         }
801                                                         else {
802                                                                 Mat3Inv(ipar_mat, arm_mat);
803                                                         }
804                                                         
805                                                         Mat3MulMat3(tempmat3, ipar_mat, pose_mat);
806                                                 }
807                                                 else {  /* we use the deform mat, for backwards compatibility */
808                                                         float imat[3][3];
809                                                         
810                                                         Mat3Inv(imat, arm_mat);
811                                                         Mat3MulMat3(tempmat3, pose_mat, imat);
812                                                 }
813                                         }
814                                         else Mat3One(tempmat3);
815                                 }
816                                 else {
817                                         float ans[4][4];
818                                         
819                                         constraint_target_to_mat4(data->tar, data->subtarget, ans, size, ctime);
820                                         /* extract rotation, is in global world coordinates */
821                                         Mat3CpyMat4(tempmat3, ans);
822                                 }
823                                 
824                                 Mat3ToEul(tempmat3, eul);
825                                 eul[0]*=(float)(180.0/M_PI);
826                                 eul[1]*=(float)(180.0/M_PI);
827                                 eul[2]*=(float)(180.0/M_PI);
828                                 
829                                 /* Target defines the animation */
830                                 s = (eul[data->type]-data->min)/(data->max-data->min);
831                                 if (s<0)
832                                         s=0;
833                                 if (s>1)
834                                         s=1;
835
836                                 t = ( s * (data->end-data->start)) + data->start;
837
838                                 /* Get the appropriate information from the action, we make temp pose */
839                                 pose = MEM_callocN(sizeof(bPose), "pose");
840                                 
841                                 pchan = ownerdata;
842                                 tchan= verify_pose_channel(pose, pchan->name);
843                                 extract_pose_from_action (pose, data->act, t);
844                                 
845                                 chan_calc_mat(tchan);
846                                 
847                                 Mat4CpyMat4(mat, tchan->chan_mat);
848
849                                 /* Clean up */
850                                 free_pose_channels(pose);
851                                 MEM_freeN(pose);
852                         }
853                         
854                 }
855                 break;
856         case CONSTRAINT_TYPE_LOCLIKE:
857                 {
858                         bLocateLikeConstraint *data = (bLocateLikeConstraint*)con->data;
859
860                         if (data->tar){
861                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
862                                 valid=1;
863                         }
864                         else
865                                 Mat4One (mat);
866                 } 
867                 break;
868         case CONSTRAINT_TYPE_MINMAX:
869                 {
870                         bMinMaxConstraint *data = (bMinMaxConstraint*)con->data;
871
872                         if (data->tar){
873                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
874                                 valid=1;
875                         }
876                         else
877                                 Mat4One (mat);
878                 } 
879                 break;
880         case CONSTRAINT_TYPE_ROTLIKE:
881                 {
882                         bRotateLikeConstraint *data;
883                         data = (bRotateLikeConstraint*)con->data;
884
885                         if (data->tar){
886                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
887                                 valid=1;
888                         }
889                         else
890                                 Mat4One (mat);
891                 } 
892                 break;
893         case CONSTRAINT_TYPE_TRACKTO:
894                 {
895                         bTrackToConstraint *data;
896                         data = (bTrackToConstraint*)con->data;
897
898                         if (data->tar){
899                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
900                                 valid=1;
901                         }
902                         else
903                                 Mat4One (mat);
904                 }
905                 break;
906         case CONSTRAINT_TYPE_KINEMATIC:
907                 {
908                         bKinematicConstraint *data;
909                         data = (bKinematicConstraint*)con->data;
910
911                         if (data->tar){
912                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
913                                 valid=1;
914                         }
915                         else if (data->flag & CONSTRAINT_IK_AUTO) {
916                                 Object *ob= ownerdata;
917                                 
918                                 if(ob==NULL)
919                                         Mat4One(mat);
920                                 else {
921                                         float vec[3];
922                                         /* move grabtarget into world space */
923                                         VECCOPY(vec, data->grabtarget);
924                                         Mat4MulVecfl(ob->obmat, vec);
925                                         Mat4CpyMat4(mat, ob->obmat);
926                                         VECCOPY(mat[3], vec);
927                                 }
928                         }
929                         else
930                                 Mat4One (mat);
931                 } 
932                 break;
933         case CONSTRAINT_TYPE_LOCKTRACK:
934                 {
935                         bLockTrackConstraint *data;
936                         data = (bLockTrackConstraint*)con->data;
937
938                         if (data->tar){
939                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
940                                 valid=1;
941                         }
942                         else
943                                 Mat4One (mat);
944                 } 
945                 break;
946         case CONSTRAINT_TYPE_FOLLOWPATH:
947                 {
948                         bFollowPathConstraint *data;
949                         data = (bFollowPathConstraint*)con->data;
950
951                         if (data->tar){
952                                 Curve *cu;
953                                 float q[4], vec[4], dir[3], *quat, x1, totmat[4][4];
954                                 float curvetime;
955
956                                 Mat4One (totmat);
957                                 Mat4One (mat);
958
959                                 cu= data->tar->data;
960
961                                 /* note; when creating constraints that follow path, the curve gets the CU_PATH set now,
962                                         currently for paths to work it needs to go through the bevlist/displist system (ton) */
963                                 
964                                 if(cu->path==NULL || cu->path->data==NULL) /* only happens on reload file, but violates depsgraph still... fix! */
965                                         makeDispListCurveTypes(data->tar, 0);
966                                 if(cu->path && cu->path->data) {
967                                         curvetime= bsystem_time(data->tar, data->tar->parent, (float)ctime, 0.0) - data->offset;
968
969                                         if(calc_ipo_spec(cu->ipo, CU_SPEED, &curvetime)==0) {
970                                                 curvetime /= cu->pathlen;
971                                                 CLAMP(curvetime, 0.0, 1.0);
972                                         }
973
974                                         if(where_on_path(data->tar, curvetime, vec, dir) ) {
975
976                                                 if(data->followflag){
977                                                         quat= vectoquat(dir, (short) data->trackflag, (short) data->upflag);
978
979                                                         Normalise(dir);
980                                                         q[0]= (float)cos(0.5*vec[3]);
981                                                         x1= (float)sin(0.5*vec[3]);
982                                                         q[1]= -x1*dir[0];
983                                                         q[2]= -x1*dir[1];
984                                                         q[3]= -x1*dir[2];
985                                                         QuatMul(quat, q, quat);
986                                                         
987
988                                                         QuatToMat4(quat, totmat);
989                                                 }
990                                                 VECCOPY(totmat[3], vec);
991
992                                                 Mat4MulSerie(mat, data->tar->obmat, totmat, NULL, NULL, NULL, NULL, NULL, NULL);
993                                         }
994                                 }
995                                 valid=1;
996                         }
997                         else
998                                 Mat4One (mat);
999                 }
1000                 break;
1001         case CONSTRAINT_TYPE_STRETCHTO:
1002                 {
1003                         bStretchToConstraint *data;
1004                         data = (bStretchToConstraint*)con->data;
1005
1006                         if (data->tar){
1007                                 constraint_target_to_mat4(data->tar, data->subtarget, mat, size, ctime);
1008                                 valid = 1;
1009                         }
1010                         else
1011                                 Mat4One (mat);
1012                 }
1013                 break;
1014
1015         default:
1016                 Mat4One(mat);
1017                 break;
1018         }
1019
1020         return valid;
1021 }
1022
1023
1024 /* only called during solve_constraints */
1025 /* bone constraints create a fake object to work on, then ob is a workob */
1026 /* if ownerdata is set, it's the posechannel */
1027 void evaluate_constraint (bConstraint *constraint, Object *ob, short ownertype, void *ownerdata, float targetmat[][4])
1028 {
1029         float   M_oldmat[4][4];
1030         float   M_identity[4][4];
1031         
1032         if (!constraint || !ob)
1033                 return;
1034         
1035         Mat4One (M_identity);
1036         
1037         switch (constraint->type){
1038         case CONSTRAINT_TYPE_ACTION:
1039                 {
1040                         float temp[4][4];
1041                         bActionConstraint *data;
1042                         
1043                         data = constraint->data;
1044                         Mat4CpyMat4 (temp, ob->obmat);
1045
1046                         Mat4MulMat4(ob->obmat, targetmat, temp);
1047                 }
1048                 break;
1049         case CONSTRAINT_TYPE_LOCLIKE:
1050                 {
1051                         bLocateLikeConstraint *data;
1052
1053                         data = constraint->data;
1054                         
1055                         if (data->flag & LOCLIKE_X)
1056                                 ob->obmat[3][0] = targetmat[3][0];
1057                         if (data->flag & LOCLIKE_Y)
1058                                 ob->obmat[3][1] = targetmat[3][1];
1059                         if (data->flag & LOCLIKE_Z)
1060                                 ob->obmat[3][2] = targetmat[3][2];
1061                 }
1062                 break;
1063         case CONSTRAINT_TYPE_ROTLIKE:
1064                 {
1065                         bRotateLikeConstraint *data;
1066                         float   tmat[3][3];
1067                         float   size[3];
1068                         
1069                         data = constraint->data;
1070                         
1071                         /* old files stuff only... version patch is too much code! */
1072                         if(data->flag==0) data->flag = ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z;
1073
1074                         Mat4ToSize(ob->obmat, size);
1075                         
1076                         Mat3CpyMat4 (tmat, targetmat);
1077                         Mat3Ortho(tmat);
1078                         
1079                         if(data->flag != (ROTLIKE_X|ROTLIKE_Y|ROTLIKE_Z)) {
1080                                 float obeul[3], eul[3], obmat[3][3];
1081                                 
1082                                 Mat3ToEul(tmat, eul);
1083                                 Mat3CpyMat4(obmat, ob->obmat);
1084                                 Mat3ToEul(obmat, obeul);
1085                                 if(!(data->flag & ROTLIKE_X)) eul[0]= obeul[0];
1086                                 if(!(data->flag & ROTLIKE_Y)) eul[1]= obeul[1];
1087                                 if(!(data->flag & ROTLIKE_Z)) eul[2]= obeul[2];
1088                                 EulToMat3(eul, tmat);
1089                         }
1090
1091                         ob->obmat[0][0] = tmat[0][0]*size[0];
1092                         ob->obmat[0][1] = tmat[0][1]*size[1];
1093                         ob->obmat[0][2] = tmat[0][2]*size[2];
1094
1095                         ob->obmat[1][0] = tmat[1][0]*size[0];
1096                         ob->obmat[1][1] = tmat[1][1]*size[1];
1097                         ob->obmat[1][2] = tmat[1][2]*size[2];
1098
1099                         ob->obmat[2][0] = tmat[2][0]*size[0];
1100                         ob->obmat[2][1] = tmat[2][1]*size[1];
1101                         ob->obmat[2][2] = tmat[2][2]*size[2];
1102                 }
1103                 break;
1104         case CONSTRAINT_TYPE_NULL:
1105                 {
1106                 }
1107                 break;
1108         case CONSTRAINT_TYPE_MINMAX:
1109                 {
1110                         float val1, val2;
1111                         int index;
1112                         bMinMaxConstraint *data;
1113
1114                         data = constraint->data;
1115
1116                         switch (data->minmaxflag){
1117                         case TRACK_Z:
1118                                 val1 = targetmat[3][2];
1119                                 val2 = ob->obmat[3][2]-data->offset;
1120                                 index = 2;
1121                                 break;
1122                         case TRACK_Y:
1123                                 val1 = targetmat[3][1];
1124                                 val2 = ob->obmat[3][1]-data->offset;
1125                                 index = 1;
1126                                 break;
1127                         case TRACK_X:
1128                                 val1 = targetmat[3][0];
1129                                 val2 = ob->obmat[3][0]-data->offset;
1130                                 index = 0;
1131                                 break;
1132                         case TRACK_nZ:
1133                                 val2 = targetmat[3][2];
1134                                 val1 = ob->obmat[3][2]-data->offset;
1135                                 index = 2;
1136                                 break;
1137                         case TRACK_nY:
1138                                 val2 = targetmat[3][1];
1139                                 val1 = ob->obmat[3][1]-data->offset;
1140                                 index = 1;
1141                                 break;
1142                         case TRACK_nX:
1143                                 val2 = targetmat[3][0];
1144                                 val1 = ob->obmat[3][0]-data->offset;
1145                                 index = 0;
1146                                 break;
1147                         default:
1148                                 return;
1149                         }
1150                         
1151                         if (val1 > val2) {
1152                                 ob->obmat[3][index] = targetmat[3][index] + data->offset;
1153                                 if (data->sticky==1) {
1154                                         if (data->stuck==1) {
1155                                                 VECCOPY(ob->obmat[3], data->cache);
1156                                         } else {
1157                                                 VECCOPY(data->cache, ob->obmat[3]);
1158                                                 data->stuck = 1;
1159                                         }
1160                                 }
1161                         } else {
1162                                 data->stuck=0;
1163                         }
1164                 }
1165                 break;
1166         case CONSTRAINT_TYPE_TRACKTO:
1167                 {
1168                         bTrackToConstraint *data;
1169                         float size[3];
1170                         float *quat;
1171                         float vec[3];
1172                         float totmat[3][3];
1173                         float tmat[4][4];
1174
1175                         data=(bTrackToConstraint*)constraint->data;                     
1176                         
1177                         if (data->tar){
1178                                         
1179                                 /* Get size property, since ob->size is only the object's own relative size, not its global one */
1180                                 Mat4ToSize (ob->obmat, size);
1181         
1182                                 Mat4CpyMat4 (M_oldmat, ob->obmat);
1183
1184                                 // Clear the object's rotation  
1185                                 ob->obmat[0][0]=size[0];
1186                                 ob->obmat[0][1]=0;
1187                                 ob->obmat[0][2]=0;
1188                                 ob->obmat[1][0]=0;
1189                                 ob->obmat[1][1]=size[1];
1190                                 ob->obmat[1][2]=0;
1191                                 ob->obmat[2][0]=0;
1192                                 ob->obmat[2][1]=0;
1193                                 ob->obmat[2][2]=size[2];
1194         
1195                         
1196                                 VecSubf(vec, ob->obmat[3], targetmat[3]);
1197                                 quat= vectoquat(vec, (short)data->reserved1, (short)data->reserved2);
1198                                 QuatToMat3(quat, totmat);
1199
1200                                 Mat4CpyMat4(tmat, ob->obmat);
1201                                 
1202                                 Mat4MulMat34(ob->obmat, totmat, tmat);
1203                         }
1204                 }
1205                 break;
1206         case CONSTRAINT_TYPE_KINEMATIC:
1207                 {
1208                         /* removed */
1209                 }
1210                 break;
1211         case CONSTRAINT_TYPE_LOCKTRACK:
1212                 {
1213                         bLockTrackConstraint *data;
1214                         float vec[3],vec2[3];
1215                         float totmat[3][3];
1216                         float tmpmat[3][3];
1217                         float invmat[3][3];
1218                         float tmat[4][4];
1219                         float mdet;
1220
1221
1222                         data=(bLockTrackConstraint*)constraint->data;                   
1223                         
1224
1225                         if (data->tar){
1226         
1227                                 Mat4CpyMat4 (M_oldmat, ob->obmat);
1228
1229                                 /* Vector object -> target */
1230                                 VecSubf(vec, targetmat[3], ob->obmat[3]);
1231                                 switch (data->lockflag){
1232                                 case LOCK_X: /* LOCK X */
1233                                         {
1234                                         switch (data->trackflag){
1235                                         case TRACK_Y: /* LOCK X TRACK Y */
1236                                                 {
1237                                                 /* Projection of Vector on the plane */
1238                                                 Projf(vec2, vec, ob->obmat[0]);
1239                                                 VecSubf(totmat[1], vec, vec2);
1240                                                 Normalise(totmat[1]);
1241
1242                                                 /* the x axis is fixed*/
1243                                                 totmat[0][0] = ob->obmat[0][0];
1244                                                 totmat[0][1] = ob->obmat[0][1];
1245                                                 totmat[0][2] = ob->obmat[0][2];
1246                                                 Normalise(totmat[0]);
1247                                 
1248                                                 /* the z axis gets mapped onto
1249                                                 a third orthogonal vector */
1250                                                 Crossf(totmat[2], totmat[0], totmat[1]);
1251                                                 }
1252                                                 break;
1253                                         case TRACK_Z: /* LOCK X TRACK Z */
1254                                                 {
1255                                                 /* Projection of Vector on the plane */
1256                                                 Projf(vec2, vec, ob->obmat[0]);
1257                                                 VecSubf(totmat[2], vec, vec2);
1258                                                 Normalise(totmat[2]);
1259
1260                                                 /* the x axis is fixed*/
1261                                                 totmat[0][0] = ob->obmat[0][0];
1262                                                 totmat[0][1] = ob->obmat[0][1];
1263                                                 totmat[0][2] = ob->obmat[0][2];
1264                                                 Normalise(totmat[0]);
1265                                 
1266                                                 /* the z axis gets mapped onto
1267                                                 a third orthogonal vector */
1268                                                 Crossf(totmat[1], totmat[2], totmat[0]);
1269                                                 }
1270                                                 break;
1271                                         case TRACK_nY: /* LOCK X TRACK -Y */
1272                                                 {
1273                                                 /* Projection of Vector on the plane */
1274                                                 Projf(vec2, vec, ob->obmat[0]);
1275                                                 VecSubf(totmat[1], vec, vec2);
1276                                                 Normalise(totmat[1]);
1277                                                 VecMulf(totmat[1],-1);
1278
1279                                                 /* the x axis is fixed*/
1280                                                 totmat[0][0] = ob->obmat[0][0];
1281                                                 totmat[0][1] = ob->obmat[0][1];
1282                                                 totmat[0][2] = ob->obmat[0][2];
1283                                                 Normalise(totmat[0]);
1284                                 
1285                                                 /* the z axis gets mapped onto
1286                                                 a third orthogonal vector */
1287                                                 Crossf(totmat[2], totmat[0], totmat[1]);
1288                                                 }
1289                                                 break;
1290                                         case TRACK_nZ: /* LOCK X TRACK -Z */
1291                                                 {
1292                                                 /* Projection of Vector on the plane */
1293                                                 Projf(vec2, vec, ob->obmat[0]);
1294                                                 VecSubf(totmat[2], vec, vec2);
1295                                                 Normalise(totmat[2]);
1296                                                 VecMulf(totmat[2],-1);
1297
1298                                                 /* the x axis is fixed*/
1299                                                 totmat[0][0] = ob->obmat[0][0];
1300                                                 totmat[0][1] = ob->obmat[0][1];
1301                                                 totmat[0][2] = ob->obmat[0][2];
1302                                                 Normalise(totmat[0]);
1303                                 
1304                                                 /* the z axis gets mapped onto
1305                                                 a third orthogonal vector */
1306                                                 Crossf(totmat[1], totmat[2], totmat[0]);
1307                                                 }
1308                                                 break;
1309                                         default:
1310                                                 {
1311                                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1312                                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1313                                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1314                                                 }
1315                                                 break;
1316                                         }
1317                                         }
1318                                         break;
1319                                 case LOCK_Y: /* LOCK Y */
1320                                         {
1321                                         switch (data->trackflag){
1322                                         case TRACK_X: /* LOCK Y TRACK X */
1323                                                 {
1324                                                 /* Projection of Vector on the plane */
1325                                                 Projf(vec2, vec, ob->obmat[1]);
1326                                                 VecSubf(totmat[0], vec, vec2);
1327                                                 Normalise(totmat[0]);
1328
1329                                                 /* the y axis is fixed*/
1330                                                 totmat[1][0] = ob->obmat[1][0];
1331                                                 totmat[1][1] = ob->obmat[1][1];
1332                                                 totmat[1][2] = ob->obmat[1][2];
1333                                                 Normalise(totmat[1]);
1334                                                 
1335                                                 /* the z axis gets mapped onto
1336                                                 a third orthogonal vector */
1337                                                 Crossf(totmat[2], totmat[0], totmat[1]);
1338                                                 }
1339                                                 break;
1340                                         case TRACK_Z: /* LOCK Y TRACK Z */
1341                                                 {
1342                                                 /* Projection of Vector on the plane */
1343                                                 Projf(vec2, vec, ob->obmat[1]);
1344                                                 VecSubf(totmat[2], vec, vec2);
1345                                                 Normalise(totmat[2]);
1346
1347                                                 /* the y axis is fixed*/
1348                                                 totmat[1][0] = ob->obmat[1][0];
1349                                                 totmat[1][1] = ob->obmat[1][1];
1350                                                 totmat[1][2] = ob->obmat[1][2];
1351                                                 Normalise(totmat[1]);
1352                                                 
1353                                                 /* the z axis gets mapped onto
1354                                                 a third orthogonal vector */
1355                                                 Crossf(totmat[0], totmat[1], totmat[2]);
1356                                                 }
1357                                                 break;
1358                                         case TRACK_nX: /* LOCK Y TRACK -X */
1359                                                 {
1360                                                 /* Projection of Vector on the plane */
1361                                                 Projf(vec2, vec, ob->obmat[1]);
1362                                                 VecSubf(totmat[0], vec, vec2);
1363                                                 Normalise(totmat[0]);
1364                                                 VecMulf(totmat[0],-1);
1365
1366                                                 /* the y axis is fixed*/
1367                                                 totmat[1][0] = ob->obmat[1][0];
1368                                                 totmat[1][1] = ob->obmat[1][1];
1369                                                 totmat[1][2] = ob->obmat[1][2];
1370                                                 Normalise(totmat[1]);
1371                                                 
1372                                                 /* the z axis gets mapped onto
1373                                                 a third orthogonal vector */
1374                                                 Crossf(totmat[2], totmat[0], totmat[1]);
1375                                                 }
1376                                                 break;
1377                                         case TRACK_nZ: /* LOCK Y TRACK -Z */
1378                                                 {
1379                                                 /* Projection of Vector on the plane */
1380                                                 Projf(vec2, vec, ob->obmat[1]);
1381                                                 VecSubf(totmat[2], vec, vec2);
1382                                                 Normalise(totmat[2]);
1383                                                 VecMulf(totmat[2],-1);
1384
1385                                                 /* the y axis is fixed*/
1386                                                 totmat[1][0] = ob->obmat[1][0];
1387                                                 totmat[1][1] = ob->obmat[1][1];
1388                                                 totmat[1][2] = ob->obmat[1][2];
1389                                                 Normalise(totmat[1]);
1390                                                 
1391                                                 /* the z axis gets mapped onto
1392                                                 a third orthogonal vector */
1393                                                 Crossf(totmat[0], totmat[1], totmat[2]);
1394                                                 }
1395                                                 break;
1396                                         default:
1397                                                 {
1398                                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1399                                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1400                                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1401                                                 }
1402                                                 break;
1403                                         }
1404                                         }
1405                                         break;
1406                                 case LOCK_Z: /* LOCK Z */
1407                                         {
1408                                         switch (data->trackflag){
1409                                         case TRACK_X: /* LOCK Z TRACK X */
1410                                                 {
1411                                                 /* Projection of Vector on the plane */
1412                                                 Projf(vec2, vec, ob->obmat[2]);
1413                                                 VecSubf(totmat[0], vec, vec2);
1414                                                 Normalise(totmat[0]);
1415
1416                                                 /* the z axis is fixed*/
1417                                                 totmat[2][0] = ob->obmat[2][0];
1418                                                 totmat[2][1] = ob->obmat[2][1];
1419                                                 totmat[2][2] = ob->obmat[2][2];
1420                                                 Normalise(totmat[2]);
1421                                                 
1422                                                 /* the x axis gets mapped onto
1423                                                 a third orthogonal vector */
1424                                                 Crossf(totmat[1], totmat[2], totmat[0]);
1425                                                 }
1426                                                 break;
1427                                         case TRACK_Y: /* LOCK Z TRACK Y */
1428                                                 {
1429                                                 /* Projection of Vector on the plane */
1430                                                 Projf(vec2, vec, ob->obmat[2]);
1431                                                 VecSubf(totmat[1], vec, vec2);
1432                                                 Normalise(totmat[1]);
1433
1434                                                 /* the z axis is fixed*/
1435                                                 totmat[2][0] = ob->obmat[2][0];
1436                                                 totmat[2][1] = ob->obmat[2][1];
1437                                                 totmat[2][2] = ob->obmat[2][2];
1438                                                 Normalise(totmat[2]);
1439                                                 
1440                                                 /* the x axis gets mapped onto
1441                                                 a third orthogonal vector */
1442                                                 Crossf(totmat[0], totmat[1], totmat[2]);
1443                                                 }
1444                                                 break;
1445                                         case TRACK_nX: /* LOCK Z TRACK -X */
1446                                                 {
1447                                                 /* Projection of Vector on the plane */
1448                                                 Projf(vec2, vec, ob->obmat[2]);
1449                                                 VecSubf(totmat[0], vec, vec2);
1450                                                 Normalise(totmat[0]);
1451                                                 VecMulf(totmat[0],-1);
1452
1453                                                 /* the z axis is fixed*/
1454                                                 totmat[2][0] = ob->obmat[2][0];
1455                                                 totmat[2][1] = ob->obmat[2][1];
1456                                                 totmat[2][2] = ob->obmat[2][2];
1457                                                 Normalise(totmat[2]);
1458                                                 
1459                                                 /* the x axis gets mapped onto
1460                                                 a third orthogonal vector */
1461                                                 Crossf(totmat[1], totmat[2], totmat[0]);
1462                                                 }
1463                                                 break;
1464                                         case TRACK_nY: /* LOCK Z TRACK -Y */
1465                                                 {
1466                                                 /* Projection of Vector on the plane */
1467                                                 Projf(vec2, vec, ob->obmat[2]);
1468                                                 VecSubf(totmat[1], vec, vec2);
1469                                                 Normalise(totmat[1]);
1470                                                 VecMulf(totmat[1],-1);
1471
1472                                                 /* the z axis is fixed*/
1473                                                 totmat[2][0] = ob->obmat[2][0];
1474                                                 totmat[2][1] = ob->obmat[2][1];
1475                                                 totmat[2][2] = ob->obmat[2][2];
1476                                                 Normalise(totmat[2]);
1477                                                 
1478                                                 /* the x axis gets mapped onto
1479                                                 a third orthogonal vector */
1480                                                 Crossf(totmat[0], totmat[1], totmat[2]);
1481                                                 }
1482                                                 break;
1483                                         default:
1484                                                 {
1485                                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1486                                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1487                                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1488                                                 }
1489                                                 break;
1490                                         }
1491                                         }
1492                                         break;
1493                                 default:
1494                                         {
1495                                                 totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1496                                                 totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1497                                                 totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1498                                         }
1499                                         break;
1500                                 }
1501                                 /* Block to keep matrix heading */
1502                                 tmpmat[0][0] = ob->obmat[0][0];tmpmat[0][1] = ob->obmat[0][1];tmpmat[0][2] = ob->obmat[0][2];
1503                                 tmpmat[1][0] = ob->obmat[1][0];tmpmat[1][1] = ob->obmat[1][1];tmpmat[1][2] = ob->obmat[1][2];
1504                                 tmpmat[2][0] = ob->obmat[2][0];tmpmat[2][1] = ob->obmat[2][1];tmpmat[2][2] = ob->obmat[2][2];
1505                                 Normalise(tmpmat[0]);
1506                                 Normalise(tmpmat[1]);
1507                                 Normalise(tmpmat[2]);
1508                                 Mat3Inv(invmat,tmpmat);
1509                                 Mat3MulMat3(tmpmat,totmat,invmat);
1510                                 totmat[0][0] = tmpmat[0][0];totmat[0][1] = tmpmat[0][1];totmat[0][2] = tmpmat[0][2];
1511                                 totmat[1][0] = tmpmat[1][0];totmat[1][1] = tmpmat[1][1];totmat[1][2] = tmpmat[1][2];
1512                                 totmat[2][0] = tmpmat[2][0];totmat[2][1] = tmpmat[2][1];totmat[2][2] = tmpmat[2][2];
1513
1514                                 Mat4CpyMat4(tmat, ob->obmat);
1515
1516                                 mdet = Det3x3(  totmat[0][0],totmat[0][1],totmat[0][2],
1517                                                                 totmat[1][0],totmat[1][1],totmat[1][2],
1518                                                                 totmat[2][0],totmat[2][1],totmat[2][2]);
1519                                 if (mdet==0)
1520                                 {
1521                                         totmat[0][0] = 1;totmat[0][1] = 0;totmat[0][2] = 0;
1522                                         totmat[1][0] = 0;totmat[1][1] = 1;totmat[1][2] = 0;
1523                                         totmat[2][0] = 0;totmat[2][1] = 0;totmat[2][2] = 1;
1524                                 }
1525
1526                                 /* apply out transformaton to the object */
1527                                 Mat4MulMat34(ob->obmat, totmat, tmat);
1528                         }
1529                 }
1530                 break;
1531         case CONSTRAINT_TYPE_FOLLOWPATH:
1532                 {
1533                         bFollowPathConstraint *data;
1534                         float obmat[4][4];
1535
1536                         data=(bFollowPathConstraint*)constraint->data;                  
1537
1538                         if (data->tar) {
1539                                 // weird, this is needed? doesnt work for workob (ton)
1540                                 object_to_mat4(ob, obmat);
1541
1542                                 Mat4MulSerie(ob->obmat, targetmat, obmat, NULL, NULL, NULL, NULL, NULL, NULL);
1543                         }
1544                 }
1545                 break;
1546         case CONSTRAINT_TYPE_STRETCHTO:
1547         {
1548             bStretchToConstraint *data;
1549             float size[3],scale[3],vec[3],xx[3],zz[3],orth[3];
1550             float totmat[3][3];
1551             float tmat[4][4];
1552             float dist;
1553             data=(bStretchToConstraint*)constraint->data;            
1554             Mat4ToSize (ob->obmat, size);
1555
1556             
1557             if (data->tar){
1558                 
1559                 /* store X orientation before destroying obmat */
1560                 xx[0] = ob->obmat[0][0];
1561                 xx[1] = ob->obmat[0][1];
1562                 xx[2] = ob->obmat[0][2];
1563                 Normalise(xx);
1564
1565                 /* store Z orientation before destroying obmat */
1566                 zz[0] = ob->obmat[2][0];
1567                 zz[1] = ob->obmat[2][1];
1568                 zz[2] = ob->obmat[2][2];
1569                 Normalise(zz);
1570
1571                                 VecSubf(vec, ob->obmat[3], targetmat[3]);
1572                                 vec[0] /= size[0];
1573                                 vec[1] /= size[1];
1574                                 vec[2] /= size[2];
1575
1576                                 dist = Normalise(vec);
1577                 //dist = VecLenf( ob->obmat[3], targetmat[3]);
1578
1579                 if (data->orglength == 0)  data->orglength = dist;
1580                 if (data->bulge ==0) data->bulge = 1.0;
1581
1582                 scale[1] = dist/data->orglength;
1583                 switch (data->volmode){
1584                 /* volume preserving scaling */
1585                 case VOLUME_XZ :
1586                     scale[0] = 1.0f - (float)sqrt(data->bulge) + (float)sqrt(data->bulge*(data->orglength/dist));
1587                     scale[2] = scale[0];
1588                     break;
1589                 case VOLUME_X:
1590                     scale[0] = 1.0f + data->bulge * (data->orglength /dist - 1);
1591                     scale[2] = 1.0;
1592                     break;
1593                 case VOLUME_Z:
1594                     scale[0] = 1.0;
1595                     scale[2] = 1.0f + data->bulge * (data->orglength /dist - 1);
1596                     break;
1597                     /* don't care for volume */
1598                 case NO_VOLUME:
1599                     scale[0] = 1.0;
1600                     scale[2] = 1.0;
1601                     break;
1602                 default: /* should not happen, but in case*/
1603                     return;    
1604                 } /* switch (data->volmode) */
1605
1606                 /* Clear the object's rotation and scale */
1607                 ob->obmat[0][0]=size[0]*scale[0];
1608                 ob->obmat[0][1]=0;
1609                 ob->obmat[0][2]=0;
1610                 ob->obmat[1][0]=0;
1611                 ob->obmat[1][1]=size[1]*scale[1];
1612                 ob->obmat[1][2]=0;
1613                 ob->obmat[2][0]=0;
1614                 ob->obmat[2][1]=0;
1615                 ob->obmat[2][2]=size[2]*scale[2];
1616                 
1617                 VecSubf(vec, ob->obmat[3], targetmat[3]);
1618                 Normalise(vec);
1619                 /* new Y aligns  object target connection*/
1620                 totmat[1][0] = -vec[0];
1621                 totmat[1][1] = -vec[1];
1622                 totmat[1][2] = -vec[2];
1623                 switch (data->plane){
1624                 case PLANE_X:
1625                     /* build new Z vector */
1626                     /* othogonal to "new Y" "old X! plane */
1627                     Crossf(orth, vec, xx);
1628                     Normalise(orth);
1629                     
1630                     /* new Z*/
1631                     totmat[2][0] = orth[0];
1632                     totmat[2][1] = orth[1];
1633                     totmat[2][2] = orth[2];
1634                     
1635                     /* we decided to keep X plane*/
1636                     Crossf(xx,orth, vec);
1637                     Normalise(xx);
1638                     totmat[0][0] = xx[0];
1639                     totmat[0][1] = xx[1];
1640                     totmat[0][2] = xx[2];
1641                     break;
1642                 case PLANE_Z:
1643                     /* build new X vector */
1644                     /* othogonal to "new Y" "old Z! plane */
1645                     Crossf(orth, vec, zz);
1646                     Normalise(orth);
1647                     
1648                     /* new X*/
1649                     totmat[0][0] = -orth[0];
1650                     totmat[0][1] = -orth[1];
1651                     totmat[0][2] = -orth[2];
1652                     
1653                     /* we decided to keep Z */
1654                     Crossf(zz,orth, vec);
1655                     Normalise(zz);
1656                     totmat[2][0] = zz[0];
1657                     totmat[2][1] = zz[1];
1658                     totmat[2][2] = zz[2];
1659                     break;
1660                 } /* switch (data->plane) */
1661                 
1662
1663                 Mat4CpyMat4(tmat, ob->obmat);
1664                 
1665                 Mat4MulMat34(ob->obmat, totmat, tmat);
1666
1667             }
1668         }
1669         break;
1670         
1671         default:
1672                 printf ("Error: Unknown constraint type\n");
1673                 break;
1674         }
1675 }