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