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 }