21cd71a90dcb2a91e869fe94c2714a20bf54117b
[blender-staging.git] / source / blender / collada / AnimationImporter.cpp
1 /*
2  * $Id$
3  *
4  * ***** BEGIN GPL 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.
10  *
11  * This program is distributed in the hope that it will be useful,
12  * but WITHOUT ANY WARRANTY; without even the implied warranty of
13  * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
14  * GNU General Public License for more details.
15  *
16  * You should have received a copy of the GNU General Public License
17  * along with this program; if not, write to the Free Software Foundation,
18  * Inc., 51 Franklin Street, Fifth Floor, Boston, MA 02110-1301, USA.
19  *
20  * Contributor(s): Chingiz Dyussenov, Arystanbek Dyussenov, Nathan Letwory.
21  *
22  * ***** END GPL LICENSE BLOCK *****
23  */
24
25 /* COLLADABU_ASSERT, may be able to remove later */
26 #include "COLLADABUPlatform.h"
27
28 #include "DNA_armature_types.h"
29
30 #include "ED_keyframing.h"
31
32 #include "BLI_listbase.h"
33 #include "BLI_math.h"
34 #include "BLI_path_util.h"
35 #include "BLI_string.h"
36
37 #include "BKE_action.h"
38 #include "BKE_armature.h"
39 #include "BKE_fcurve.h"
40 #include "BKE_object.h"
41
42 #include "MEM_guardedalloc.h"
43
44 #include "collada_utils.h"
45 #include "AnimationImporter.h"
46 #include "ArmatureImporter.h"
47
48 #include <algorithm>
49
50 // use this for retrieving bone names, since these must be unique
51 template<class T>
52 static const char *bc_get_joint_name(T *node)
53 {
54         const std::string& id = node->getOriginalId();
55         return id.size() ? id.c_str() : node->getName().c_str();
56 }
57
58 FCurve *AnimationImporter::create_fcurve(int array_index, const char *rna_path)
59 {
60         FCurve *fcu = (FCurve*)MEM_callocN(sizeof(FCurve), "FCurve");
61         
62         fcu->flag = (FCURVE_VISIBLE|FCURVE_AUTO_HANDLES|FCURVE_SELECTED);
63         fcu->rna_path = BLI_strdupn(rna_path, strlen(rna_path));
64         fcu->array_index = array_index;
65         return fcu;
66 }
67         
68 void AnimationImporter::create_bezt(FCurve *fcu, float frame, float output)
69 {
70         BezTriple bez;
71         memset(&bez, 0, sizeof(BezTriple));
72         bez.vec[1][0] = frame;
73         bez.vec[1][1] = output;
74         bez.ipo = U.ipo_new; /* use default interpolation mode here... */
75         bez.f1 = bez.f2 = bez.f3 = SELECT;
76         bez.h1 = bez.h2 = HD_AUTO;
77         insert_bezt_fcurve(fcu, &bez, 0);
78         calchandles_fcurve(fcu);
79 }
80
81 // create one or several fcurves depending on the number of parameters being animated
82 void AnimationImporter::animation_to_fcurves(COLLADAFW::AnimationCurve *curve)
83 {
84         COLLADAFW::FloatOrDoubleArray& input = curve->getInputValues();
85         COLLADAFW::FloatOrDoubleArray& output = curve->getOutputValues();
86         // COLLADAFW::FloatOrDoubleArray& intan = curve->getInTangentValues();
87         // COLLADAFW::FloatOrDoubleArray& outtan = curve->getOutTangentValues();
88         float fps = (float)FPS;
89         size_t dim = curve->getOutDimension();
90         unsigned int i;
91
92         std::vector<FCurve*>& fcurves = curve_map[curve->getUniqueId()];
93
94         switch (dim) {
95         case 1: // X, Y, Z or angle
96         case 3: // XYZ
97         case 16: // matrix
98                 {
99                         for (i = 0; i < dim; i++ ) {
100                                 FCurve *fcu = (FCurve*)MEM_callocN(sizeof(FCurve), "FCurve");
101                         
102                                 fcu->flag = (FCURVE_VISIBLE|FCURVE_AUTO_HANDLES|FCURVE_SELECTED);
103                                 // fcu->rna_path = BLI_strdupn(path, strlen(path));
104                                 fcu->array_index = 0;
105                                 //fcu->totvert = curve->getKeyCount();
106                         
107                                 // create beztriple for each key
108                                 for (unsigned int j = 0; j < curve->getKeyCount(); j++) {
109                                         BezTriple bez;
110                                         memset(&bez, 0, sizeof(BezTriple));
111
112                                         // intangent
113                                         // bez.vec[0][0] = get_float_value(intan, j * 6 + i + i) * fps;
114                                         // bez.vec[0][1] = get_float_value(intan, j * 6 + i + i + 1);
115
116                                         // input, output
117                                         bez.vec[1][0] = bc_get_float_value(input, j) * fps; 
118                                         bez.vec[1][1] = bc_get_float_value(output, j * dim + i);
119
120                                         // outtangent
121                                         // bez.vec[2][0] = get_float_value(outtan, j * 6 + i + i) * fps;
122                                         // bez.vec[2][1] = get_float_value(outtan, j * 6 + i + i + 1);
123
124                                         bez.ipo = U.ipo_new; /* use default interpolation mode here... */
125                                         bez.f1 = bez.f2 = bez.f3 = SELECT;
126                                         bez.h1 = bez.h2 = HD_AUTO;
127                                         insert_bezt_fcurve(fcu, &bez, 0);
128                                 }
129
130                                 calchandles_fcurve(fcu);
131
132                                 fcurves.push_back(fcu);
133                         }
134                 }
135                 break;
136         default:
137                 fprintf(stderr, "Output dimension of %d is not yet supported (animation id = %s)\n", (int)dim, curve->getOriginalId().c_str());
138         }
139
140         for (std::vector<FCurve*>::iterator it = fcurves.begin(); it != fcurves.end(); it++)
141                 unused_curves.push_back(*it);
142 }
143
144 void AnimationImporter::fcurve_deg_to_rad(FCurve *cu)
145 {
146         for (unsigned int i = 0; i < cu->totvert; i++) {
147                 // TODO convert handles too
148                 cu->bezt[i].vec[1][1] *= M_PI / 180.0f;
149         }
150 }
151
152 void AnimationImporter::add_fcurves_to_object(Object *ob, std::vector<FCurve*>& curves, char *rna_path, int array_index, Animation *animated)
153 {
154         bAction *act;
155         
156         if (!ob->adt || !ob->adt->action) act = verify_adt_action((ID*)&ob->id, 1);
157         else act = ob->adt->action;
158         
159         std::vector<FCurve*>::iterator it;
160         int i;
161
162 #if 0
163         char *p = strstr(rna_path, "rotation_euler");
164         bool is_rotation = p && *(p + strlen("rotation_euler")) == '\0';
165
166         // convert degrees to radians for rotation
167         if (is_rotation)
168                 fcurve_deg_to_rad(fcu);
169 #endif
170         
171         for (it = curves.begin(), i = 0; it != curves.end(); it++, i++) {
172                 FCurve *fcu = *it;
173                 fcu->rna_path = BLI_strdupn(rna_path, strlen(rna_path));
174                 
175                 if (array_index == -1) fcu->array_index = i;
176                 else fcu->array_index = array_index;
177         
178                 if (ob->type == OB_ARMATURE) {
179                         bActionGroup *grp = NULL;
180                         const char *bone_name = bc_get_joint_name(animated->node);
181                         
182                         if (bone_name) {
183                                 /* try to find group */
184                                 grp = action_groups_find_named(act, bone_name);
185                                 
186                                 /* no matching groups, so add one */
187                                 if (grp == NULL) {
188                                         /* Add a new group, and make it active */
189                                         grp = (bActionGroup*)MEM_callocN(sizeof(bActionGroup), "bActionGroup");
190                                         
191                                         grp->flag = AGRP_SELECTED;
192                                         BLI_strncpy(grp->name, bone_name, sizeof(grp->name));
193                                         
194                                         BLI_addtail(&act->groups, grp);
195                                         BLI_uniquename(&act->groups, grp, "Group", '.', offsetof(bActionGroup, name), 64);
196                                 }
197                                 
198                                 /* add F-Curve to group */
199                                 action_groups_add_channel(act, grp, fcu);
200                                 
201                         }
202 #if 0
203                         if (is_rotation) {
204                                 fcurves_actionGroup_map[grp].push_back(fcu);
205                         }
206 #endif
207                 }
208                 else {
209                         BLI_addtail(&act->curves, fcu);
210                 }
211
212                 // curve is used, so remove it from unused_curves
213                 unused_curves.erase(std::remove(unused_curves.begin(), unused_curves.end(), fcu), unused_curves.end());
214         }
215 }
216
217 AnimationImporter::AnimationImporter(UnitConverter *conv, ArmatureImporter *arm, Scene *scene) :
218                 TransformReader(conv), armature_importer(arm), scene(scene) { }
219
220 AnimationImporter::~AnimationImporter()
221 {
222         // free unused FCurves
223         for (std::vector<FCurve*>::iterator it = unused_curves.begin(); it != unused_curves.end(); it++)
224                 free_fcurve(*it);
225
226         if (unused_curves.size())
227                 fprintf(stderr, "removed %d unused curves\n", (int)unused_curves.size());
228 }
229
230 bool AnimationImporter::write_animation(const COLLADAFW::Animation* anim) 
231 {
232         if (anim->getAnimationType() == COLLADAFW::Animation::ANIMATION_CURVE) {
233                 COLLADAFW::AnimationCurve *curve = (COLLADAFW::AnimationCurve*)anim;
234                 
235                 // XXX Don't know if it's necessary
236                 // Should we check outPhysicalDimension?
237                 if (curve->getInPhysicalDimension() != COLLADAFW::PHYSICAL_DIMENSION_TIME) {
238                         fprintf(stderr, "Inputs physical dimension is not time. \n");
239                         return true;
240                 }
241
242                 // a curve can have mixed interpolation type,
243                 // in this case curve->getInterpolationTypes returns a list of interpolation types per key
244                 COLLADAFW::AnimationCurve::InterpolationType interp = curve->getInterpolationType();
245
246                 if (interp != COLLADAFW::AnimationCurve::INTERPOLATION_MIXED) {
247                         switch (interp) {
248                         case COLLADAFW::AnimationCurve::INTERPOLATION_LINEAR:
249                         case COLLADAFW::AnimationCurve::INTERPOLATION_BEZIER:
250                                 animation_to_fcurves(curve);
251                                 break;
252                         default:
253                                 // TODO there're also CARDINAL, HERMITE, BSPLINE and STEP types
254                                 fprintf(stderr, "CARDINAL, HERMITE, BSPLINE and STEP anim interpolation types not supported yet.\n");
255                                 break;
256                         }
257                 }
258                 else {
259                         // not supported yet
260                         fprintf(stderr, "MIXED anim interpolation type is not supported yet.\n");
261                 }
262         }
263         else {
264                 fprintf(stderr, "FORMULA animation type is not supported yet.\n");
265         }
266         
267         return true;
268 }
269         
270 // called on post-process stage after writeVisualScenes
271 bool AnimationImporter::write_animation_list(const COLLADAFW::AnimationList* animlist) 
272 {
273         const COLLADAFW::UniqueId& animlist_id = animlist->getUniqueId();
274
275         animlist_map[animlist_id] = animlist;
276
277 #if 0
278         // should not happen
279         if (uid_animated_map.find(animlist_id) == uid_animated_map.end()) {
280                 return true;
281         }
282
283         // for bones rna_path is like: pose.bones["bone-name"].rotation
284         
285         // what does this AnimationList animate?
286         Animation& animated = uid_animated_map[animlist_id];
287         Object *ob = animated.ob;
288
289         char rna_path[100];
290         char joint_path[100];
291         bool is_joint = false;
292
293         // if ob is NULL, it should be a JOINT
294         if (!ob) {
295                 ob = armature_importer->get_armature_for_joint(animated.node);
296
297                 if (!ob) {
298                         fprintf(stderr, "Cannot find armature for node %s\n", get_joint_name(animated.node));
299                         return true;
300                 }
301
302                 armature_importer->get_rna_path_for_joint(animated.node, joint_path, sizeof(joint_path));
303
304                 is_joint = true;
305         }
306         
307         const COLLADAFW::AnimationList::AnimationBindings& bindings = animlist->getAnimationBindings();
308
309         switch (animated.tm->getTransformationType()) {
310         case COLLADAFW::Transformation::TRANSLATE:
311         case COLLADAFW::Transformation::SCALE:
312                 {
313                         bool loc = animated.tm->getTransformationType() == COLLADAFW::Transformation::TRANSLATE;
314                         if (is_joint)
315                                 BLI_snprintf(rna_path, sizeof(rna_path), "%s.%s", joint_path, loc ? "location" : "scale");
316                         else
317                                 BLI_strncpy(rna_path, loc ? "location" : "scale", sizeof(rna_path));
318
319                         for (int i = 0; i < bindings.getCount(); i++) {
320                                 const COLLADAFW::AnimationList::AnimationBinding& binding = bindings[i];
321                                 COLLADAFW::UniqueId anim_uid = binding.animation;
322
323                                 if (curve_map.find(anim_uid) == curve_map.end()) {
324                                         fprintf(stderr, "Cannot find FCurve by animation UID.\n");
325                                         continue;
326                                 }
327
328                                 std::vector<FCurve*>& fcurves = curve_map[anim_uid];
329                                 
330                                 switch (binding.animationClass) {
331                                 case COLLADAFW::AnimationList::POSITION_X:
332                                         add_fcurves_to_object(ob, fcurves, rna_path, 0, &animated);
333                                         break;
334                                 case COLLADAFW::AnimationList::POSITION_Y:
335                                         add_fcurves_to_object(ob, fcurves, rna_path, 1, &animated);
336                                         break;
337                                 case COLLADAFW::AnimationList::POSITION_Z:
338                                         add_fcurves_to_object(ob, fcurves, rna_path, 2, &animated);
339                                         break;
340                                 case COLLADAFW::AnimationList::POSITION_XYZ:
341                                         add_fcurves_to_object(ob, fcurves, rna_path, -1, &animated);
342                                         break;
343                                 default:
344                                         fprintf(stderr, "AnimationClass %d is not supported for %s.\n",
345                                                         binding.animationClass, loc ? "TRANSLATE" : "SCALE");
346                                 }
347                         }
348                 }
349                 break;
350         case COLLADAFW::Transformation::ROTATE:
351                 {
352                         if (is_joint)
353                                 BLI_snprintf(rna_path, sizeof(rna_path), "%s.rotation_euler", joint_path);
354                         else
355                                 BLI_strncpy(rna_path, "rotation_euler", sizeof(rna_path));
356
357                         COLLADAFW::Rotate* rot = (COLLADAFW::Rotate*)animated.tm;
358                         COLLADABU::Math::Vector3& axis = rot->getRotationAxis();
359                         
360                         for (int i = 0; i < bindings.getCount(); i++) {
361                                 const COLLADAFW::AnimationList::AnimationBinding& binding = bindings[i];
362                                 COLLADAFW::UniqueId anim_uid = binding.animation;
363
364                                 if (curve_map.find(anim_uid) == curve_map.end()) {
365                                         fprintf(stderr, "Cannot find FCurve by animation UID.\n");
366                                         continue;
367                                 }
368
369                                 std::vector<FCurve*>& fcurves = curve_map[anim_uid];
370
371                                 switch (binding.animationClass) {
372                                 case COLLADAFW::AnimationList::ANGLE:
373                                         if (COLLADABU::Math::Vector3::UNIT_X == axis) {
374                                                 add_fcurves_to_object(ob, fcurves, rna_path, 0, &animated);
375                                         }
376                                         else if (COLLADABU::Math::Vector3::UNIT_Y == axis) {
377                                                 add_fcurves_to_object(ob, fcurves, rna_path, 1, &animated);
378                                         }
379                                         else if (COLLADABU::Math::Vector3::UNIT_Z == axis) {
380                                                 add_fcurves_to_object(ob, fcurves, rna_path, 2, &animated);
381                                         }
382                                         break;
383                                 case COLLADAFW::AnimationList::AXISANGLE:
384                                         // TODO convert axis-angle to quat? or XYZ?
385                                 default:
386                                         fprintf(stderr, "AnimationClass %d is not supported for ROTATE transformation.\n",
387                                                         binding.animationClass);
388                                 }
389                         }
390                 }
391                 break;
392         case COLLADAFW::Transformation::MATRIX:
393         case COLLADAFW::Transformation::SKEW:
394         case COLLADAFW::Transformation::LOOKAT:
395                 fprintf(stderr, "Animation of MATRIX, SKEW and LOOKAT transformations is not supported yet.\n");
396                 break;
397         }
398 #endif
399         
400         return true;
401 }
402
403 void AnimationImporter::read_node_transform(COLLADAFW::Node *node, Object *ob)
404 {
405         float mat[4][4];
406         TransformReader::get_node_mat(mat, node, &uid_animated_map, ob);
407         if (ob) {
408                 copy_m4_m4(ob->obmat, mat);
409                 object_apply_mat4(ob, ob->obmat, 0, 0);
410         }
411 }
412
413 #if 0
414 virtual void AnimationImporter::change_eul_to_quat(Object *ob, bAction *act)
415 {
416         bActionGroup *grp;
417         int i;
418         
419         for (grp = (bActionGroup*)act->groups.first; grp; grp = grp->next) {
420
421                 FCurve *eulcu[3] = {NULL, NULL, NULL};
422                 
423                 if (fcurves_actionGroup_map.find(grp) == fcurves_actionGroup_map.end())
424                         continue;
425
426                 std::vector<FCurve*> &rot_fcurves = fcurves_actionGroup_map[grp];
427                 
428                 if (rot_fcurves.size() > 3) continue;
429
430                 for (i = 0; i < rot_fcurves.size(); i++)
431                         eulcu[rot_fcurves[i]->array_index] = rot_fcurves[i];
432
433                 char joint_path[100];
434                 char rna_path[100];
435
436                 BLI_snprintf(joint_path, sizeof(joint_path), "pose.bones[\"%s\"]", grp->name);
437                 BLI_snprintf(rna_path, sizeof(rna_path), "%s.rotation_quaternion", joint_path);
438
439                 FCurve *quatcu[4] = {
440                         create_fcurve(0, rna_path),
441                         create_fcurve(1, rna_path),
442                         create_fcurve(2, rna_path),
443                         create_fcurve(3, rna_path)
444                 };
445
446                 bPoseChannel *chan = get_pose_channel(ob->pose, grp->name);
447
448                 float m4[4][4], irest[3][3];
449                 invert_m4_m4(m4, chan->bone->arm_mat);
450                 copy_m3_m4(irest, m4);
451
452                 for (i = 0; i < 3; i++) {
453
454                         FCurve *cu = eulcu[i];
455
456                         if (!cu) continue;
457
458                         for (int j = 0; j < cu->totvert; j++) {
459                                 float frame = cu->bezt[j].vec[1][0];
460
461                                 float eul[3] = {
462                                         eulcu[0] ? evaluate_fcurve(eulcu[0], frame) : 0.0f,
463                                         eulcu[1] ? evaluate_fcurve(eulcu[1], frame) : 0.0f,
464                                         eulcu[2] ? evaluate_fcurve(eulcu[2], frame) : 0.0f
465                                 };
466
467                                 // make eul relative to bone rest pose
468                                 float rot[3][3], rel[3][3], quat[4];
469
470                                 /*eul_to_mat3(rot, eul);
471
472                                 mul_m3_m3m3(rel, irest, rot);
473
474                                 mat3_to_quat(quat, rel);
475                                 */
476
477                                 eul_to_quat(quat, eul);
478
479                                 for (int k = 0; k < 4; k++)
480                                         create_bezt(quatcu[k], frame, quat[k]);
481                         }
482                 }
483
484                 // now replace old Euler curves
485
486                 for (i = 0; i < 3; i++) {
487                         if (!eulcu[i]) continue;
488
489                         action_groups_remove_channel(act, eulcu[i]);
490                         free_fcurve(eulcu[i]);
491                 }
492
493                 chan->rotmode = ROT_MODE_QUAT;
494
495                 for (i = 0; i < 4; i++)
496                         action_groups_add_channel(act, grp, quatcu[i]);
497         }
498
499         bPoseChannel *pchan;
500         for (pchan = (bPoseChannel*)ob->pose->chanbase.first; pchan; pchan = pchan->next) {
501                 pchan->rotmode = ROT_MODE_QUAT;
502         }
503 }
504 #endif
505
506 // prerequisites:
507 // animlist_map - map animlist id -> animlist
508 // curve_map - map anim id -> curve(s)
509 Object *AnimationImporter::translate_animation(COLLADAFW::Node *node,
510                                                         std::map<COLLADAFW::UniqueId, Object*>& object_map,
511                                                         std::map<COLLADAFW::UniqueId, COLLADAFW::Node*>& root_map,
512                                                         COLLADAFW::Transformation::TransformationType tm_type,
513                                                         Object *par_job)
514 {
515         bool is_rotation = tm_type == COLLADAFW::Transformation::ROTATE;
516         bool is_matrix = tm_type == COLLADAFW::Transformation::MATRIX;
517         bool is_joint = node->getType() == COLLADAFW::Node::JOINT;
518
519         COLLADAFW::Node *root = root_map.find(node->getUniqueId()) == root_map.end() ? node : root_map[node->getUniqueId()];
520         Object *ob = is_joint ? armature_importer->get_armature_for_joint(node) : object_map[node->getUniqueId()];
521         const char *bone_name = is_joint ? bc_get_joint_name(node) : NULL;
522
523         if (!ob) {
524                 fprintf(stderr, "cannot find Object for Node with id=\"%s\"\n", node->getOriginalId().c_str());
525                 return NULL;
526         }
527
528         // frames at which to sample
529         std::vector<float> frames;
530
531         // for each <rotate>, <translate>, etc. there is a separate Transformation
532         const COLLADAFW::TransformationPointerArray& tms = node->getTransformations();
533
534         unsigned int i;
535
536         // find frames at which to sample plus convert all rotation keys to radians
537         for (i = 0; i < tms.getCount(); i++) {
538                 COLLADAFW::Transformation *tm = tms[i];
539                 COLLADAFW::Transformation::TransformationType type = tm->getTransformationType();
540
541                 if (type == tm_type) {
542                         const COLLADAFW::UniqueId& listid = tm->getAnimationList();
543
544                         if (animlist_map.find(listid) != animlist_map.end()) {
545                                 const COLLADAFW::AnimationList *animlist = animlist_map[listid];
546                                 const COLLADAFW::AnimationList::AnimationBindings& bindings = animlist->getAnimationBindings();
547
548                                 if (bindings.getCount()) {
549                                         for (unsigned int j = 0; j < bindings.getCount(); j++) {
550                                                 std::vector<FCurve*>& curves = curve_map[bindings[j].animation];
551                                                 bool xyz = ((type == COLLADAFW::Transformation::TRANSLATE || type == COLLADAFW::Transformation::SCALE) && bindings[j].animationClass == COLLADAFW::AnimationList::POSITION_XYZ);
552
553                                                 if ((!xyz && curves.size() == 1) || (xyz && curves.size() == 3) || is_matrix) {
554                                                         std::vector<FCurve*>::iterator iter;
555
556                                                         for (iter = curves.begin(); iter != curves.end(); iter++) {
557                                                                 FCurve *fcu = *iter;
558
559                                                                 if (is_rotation)
560                                                                         fcurve_deg_to_rad(fcu);
561
562                                                                 for (unsigned int k = 0; k < fcu->totvert; k++) {
563                                                                         float fra = fcu->bezt[k].vec[1][0];
564                                                                         if (std::find(frames.begin(), frames.end(), fra) == frames.end())
565                                                                                 frames.push_back(fra);
566                                                                 }
567                                                         }
568                                                 }
569                                                 else {
570                                                         fprintf(stderr, "expected %d curves, got %d\n", xyz ? 3 : 1, (int)curves.size());
571                                                 }
572                                         }
573                                 }
574                         }
575                 }
576         }
577
578         float irest_dae[4][4];
579         float rest[4][4], irest[4][4];
580
581         if (is_joint) {
582                 get_joint_rest_mat(irest_dae, root, node);
583                 invert_m4(irest_dae);
584
585                 Bone *bone = get_named_bone((bArmature*)ob->data, bone_name);
586                 if (!bone) {
587                         fprintf(stderr, "cannot find bone \"%s\"\n", bone_name);
588                         return NULL;
589                 }
590
591                 unit_m4(rest);
592                 copy_m4_m4(rest, bone->arm_mat);
593                 invert_m4_m4(irest, rest);
594         }
595
596         Object *job = NULL;
597
598 #ifdef ARMATURE_TEST
599         FCurve *job_curves[10];
600         job = get_joint_object(root, node, par_job);
601 #endif
602
603         if (frames.size() == 0)
604                 return job;
605
606         std::sort(frames.begin(), frames.end());
607
608         const char *tm_str = NULL;
609         switch (tm_type) {
610         case COLLADAFW::Transformation::ROTATE:
611                 tm_str = "rotation_quaternion";
612                 break;
613         case COLLADAFW::Transformation::SCALE:
614                 tm_str = "scale";
615                 break;
616         case COLLADAFW::Transformation::TRANSLATE:
617                 tm_str = "location";
618                 break;
619         case COLLADAFW::Transformation::MATRIX:
620                 break;
621         default:
622                 return job;
623         }
624
625         char rna_path[200];
626         char joint_path[200];
627
628         if (is_joint)
629                 armature_importer->get_rna_path_for_joint(node, joint_path, sizeof(joint_path));
630
631         // new curves
632         FCurve *newcu[10]; // if tm_type is matrix, then create 10 curves: 4 rot, 3 loc, 3 scale
633         unsigned int totcu = is_matrix ? 10 : (is_rotation ? 4 : 3);
634
635         for (i = 0; i < totcu; i++) {
636
637                 int axis = i;
638
639                 if (is_matrix) {
640                         if (i < 4) {
641                                 tm_str = "rotation_quaternion";
642                                 axis = i;
643                         }
644                         else if (i < 7) {
645                                 tm_str = "location";
646                                 axis = i - 4;
647                         }
648                         else {
649                                 tm_str = "scale";
650                                 axis = i - 7;
651                         }
652                 }
653
654                 if (is_joint)
655                         BLI_snprintf(rna_path, sizeof(rna_path), "%s.%s", joint_path, tm_str);
656                 else
657                         strcpy(rna_path, tm_str);
658
659                 newcu[i] = create_fcurve(axis, rna_path);
660
661 #ifdef ARMATURE_TEST
662                 if (is_joint)
663                         job_curves[i] = create_fcurve(axis, tm_str);
664 #endif
665         }
666
667         std::vector<float>::iterator it;
668
669         // sample values at each frame
670         for (it = frames.begin(); it != frames.end(); it++) {
671                 float fra = *it;
672
673                 float mat[4][4];
674                 float matfra[4][4];
675
676                 unit_m4(matfra);
677
678                 // calc object-space mat
679                 evaluate_transform_at_frame(matfra, node, fra);
680
681                 // for joints, we need a special matrix
682                 if (is_joint) {
683                         // special matrix: iR * M * iR_dae * R
684                         // where R, iR are bone rest and inverse rest mats in world space (Blender bones),
685                         // iR_dae is joint inverse rest matrix (DAE) and M is an evaluated joint world-space matrix (DAE)
686                         float temp[4][4], par[4][4];
687
688                         // calc M
689                         calc_joint_parent_mat_rest(par, NULL, root, node);
690                         mul_m4_m4m4(temp, matfra, par);
691
692                         // evaluate_joint_world_transform_at_frame(temp, NULL, , node, fra);
693
694                         // calc special matrix
695                         mul_serie_m4(mat, irest, temp, irest_dae, rest, NULL, NULL, NULL, NULL);
696                 }
697                 else {
698                         copy_m4_m4(mat, matfra);
699                 }
700
701                 float val[4], rot[4], loc[3], scale[3];
702
703                 switch (tm_type) {
704                 case COLLADAFW::Transformation::ROTATE:
705                         mat4_to_quat(val, mat);
706                         break;
707                 case COLLADAFW::Transformation::SCALE:
708                         mat4_to_size(val, mat);
709                         break;
710                 case COLLADAFW::Transformation::TRANSLATE:
711                         copy_v3_v3(val, mat[3]);
712                         break;
713                 case COLLADAFW::Transformation::MATRIX:
714                         mat4_to_quat(rot, mat);
715                         copy_v3_v3(loc, mat[3]);
716                         mat4_to_size(scale, mat);
717                         break;
718                 default:
719                         break;
720                 }
721
722                 // add keys
723                 for (i = 0; i < totcu; i++) {
724                         if (is_matrix) {
725                                 if (i < 4)
726                                         add_bezt(newcu[i], fra, rot[i]);
727                                 else if (i < 7)
728                                         add_bezt(newcu[i], fra, loc[i - 4]);
729                                 else
730                                         add_bezt(newcu[i], fra, scale[i - 7]);
731                         }
732                         else {
733                                 add_bezt(newcu[i], fra, val[i]);
734                         }
735                 }
736
737 #ifdef ARMATURE_TEST
738                 if (is_joint) {
739                         switch (tm_type) {
740                         case COLLADAFW::Transformation::ROTATE:
741                                 mat4_to_quat(val, matfra);
742                                 break;
743                         case COLLADAFW::Transformation::SCALE:
744                                 mat4_to_size(val, matfra);
745                                 break;
746                         case COLLADAFW::Transformation::TRANSLATE:
747                                 copy_v3_v3(val, matfra[3]);
748                                 break;
749                         case MATRIX:
750                                 mat4_to_quat(rot, matfra);
751                                 copy_v3_v3(loc, matfra[3]);
752                                 mat4_to_size(scale, matfra);
753                                 break;
754                         default:
755                                 break;
756                         }
757
758                         for (i = 0; i < totcu; i++) {
759                                 if (is_matrix) {
760                                         if (i < 4)
761                                                 add_bezt(job_curves[i], fra, rot[i]);
762                                         else if (i < 7)
763                                                 add_bezt(job_curves[i], fra, loc[i - 4]);
764                                         else
765                                                 add_bezt(job_curves[i], fra, scale[i - 7]);
766                                 }
767                                 else {
768                                         add_bezt(job_curves[i], fra, val[i]);
769                                 }
770                         }
771                 }
772 #endif
773         }
774
775         verify_adt_action((ID*)&ob->id, 1);
776
777         ListBase *curves = &ob->adt->action->curves;
778
779         // add curves
780         for (i = 0; i < totcu; i++) {
781                 if (is_joint)
782                         add_bone_fcurve(ob, node, newcu[i]);
783                 else
784                         BLI_addtail(curves, newcu[i]);
785
786 #ifdef ARMATURE_TEST
787                 if (is_joint)
788                         BLI_addtail(&job->adt->action->curves, job_curves[i]);
789 #endif
790         }
791
792         if (is_rotation || is_matrix) {
793                 if (is_joint) {
794                         bPoseChannel *chan = get_pose_channel(ob->pose, bone_name);
795                         chan->rotmode = ROT_MODE_QUAT;
796                 }
797                 else {
798                         ob->rotmode = ROT_MODE_QUAT;
799                 }
800         }
801
802         return job;
803 }
804
805 // internal, better make it private
806 // warning: evaluates only rotation
807 // prerequisites: animlist_map, curve_map
808 void AnimationImporter::evaluate_transform_at_frame(float mat[4][4], COLLADAFW::Node *node, float fra)
809 {
810         const COLLADAFW::TransformationPointerArray& tms = node->getTransformations();
811
812         unit_m4(mat);
813
814         for (unsigned int i = 0; i < tms.getCount(); i++) {
815                 COLLADAFW::Transformation *tm = tms[i];
816                 COLLADAFW::Transformation::TransformationType type = tm->getTransformationType();
817                 float m[4][4];
818
819                 unit_m4(m);
820
821                 if (!evaluate_animation(tm, m, fra, node->getOriginalId().c_str())) {
822                         switch (type) {
823                         case COLLADAFW::Transformation::ROTATE:
824                                 dae_rotate_to_mat4(tm, m);
825                                 break;
826                         case COLLADAFW::Transformation::TRANSLATE:
827                                 dae_translate_to_mat4(tm, m);
828                                 break;
829                         case COLLADAFW::Transformation::SCALE:
830                                 dae_scale_to_mat4(tm, m);
831                                 break;
832                         case COLLADAFW::Transformation::MATRIX:
833                                 dae_matrix_to_mat4(tm, m);
834                                 break;
835                         default:
836                                 fprintf(stderr, "unsupported transformation type %d\n", type);
837                         }
838                 }
839
840                 float temp[4][4];
841                 copy_m4_m4(temp, mat);
842
843                 mul_m4_m4m4(mat, m, temp);
844         }
845 }
846
847 // return true to indicate that mat contains a sane value
848 bool AnimationImporter::evaluate_animation(COLLADAFW::Transformation *tm, float mat[4][4], float fra, const char *node_id)
849 {
850         const COLLADAFW::UniqueId& listid = tm->getAnimationList();
851         COLLADAFW::Transformation::TransformationType type = tm->getTransformationType();
852
853         if (type != COLLADAFW::Transformation::ROTATE &&
854             type != COLLADAFW::Transformation::SCALE &&
855             type != COLLADAFW::Transformation::TRANSLATE &&
856             type != COLLADAFW::Transformation::MATRIX) {
857                 fprintf(stderr, "animation of transformation %d is not supported yet\n", type);
858                 return false;
859         }
860
861         if (animlist_map.find(listid) == animlist_map.end())
862                 return false;
863
864         const COLLADAFW::AnimationList *animlist = animlist_map[listid];
865         const COLLADAFW::AnimationList::AnimationBindings& bindings = animlist->getAnimationBindings();
866
867         if (bindings.getCount()) {
868                 float vec[3];
869
870                 bool is_scale = (type == COLLADAFW::Transformation::SCALE);
871                 bool is_translate = (type == COLLADAFW::Transformation::TRANSLATE);
872
873                 if (type == COLLADAFW::Transformation::SCALE)
874                         dae_scale_to_v3(tm, vec);
875                 else if (type == COLLADAFW::Transformation::TRANSLATE)
876                         dae_translate_to_v3(tm, vec);
877
878                 for (unsigned int j = 0; j < bindings.getCount(); j++) {
879                         const COLLADAFW::AnimationList::AnimationBinding& binding = bindings[j];
880                         std::vector<FCurve*>& curves = curve_map[binding.animation];
881                         COLLADAFW::AnimationList::AnimationClass animclass = binding.animationClass;
882                         char path[100];
883
884                         switch (type) {
885                         case COLLADAFW::Transformation::ROTATE:
886                                 BLI_snprintf(path, sizeof(path), "%s.rotate (binding %u)", node_id, j);
887                                 break;
888                         case COLLADAFW::Transformation::SCALE:
889                                 BLI_snprintf(path, sizeof(path), "%s.scale (binding %u)", node_id, j);
890                                 break;
891                         case COLLADAFW::Transformation::TRANSLATE:
892                                 BLI_snprintf(path, sizeof(path), "%s.translate (binding %u)", node_id, j);
893                                 break;
894                         case COLLADAFW::Transformation::MATRIX:
895                                 BLI_snprintf(path, sizeof(path), "%s.matrix (binding %u)", node_id, j);
896                                 break;
897                         default:
898                                 break;
899                         }
900
901                         if (animclass == COLLADAFW::AnimationList::UNKNOWN_CLASS) {
902                                 fprintf(stderr, "%s: UNKNOWN animation class\n", path);
903                                 continue;
904                         }
905
906                         if (type == COLLADAFW::Transformation::ROTATE) {
907                                 if (curves.size() != 1) {
908                                         fprintf(stderr, "expected 1 curve, got %d\n", (int)curves.size());
909                                         return false;
910                                 }
911
912                                 // TODO support other animclasses
913                                 if (animclass != COLLADAFW::AnimationList::ANGLE) {
914                                         fprintf(stderr, "%s: animation class %d is not supported yet\n", path, animclass);
915                                         return false;
916                                 }
917
918                                 COLLADABU::Math::Vector3& axis = ((COLLADAFW::Rotate*)tm)->getRotationAxis();
919                                 float ax[3] = {axis[0], axis[1], axis[2]};
920                                 float angle = evaluate_fcurve(curves[0], fra);
921                                 axis_angle_to_mat4(mat, ax, angle);
922
923                                 return true;
924                         }
925                         else if (is_scale || is_translate) {
926                                 bool is_xyz = animclass == COLLADAFW::AnimationList::POSITION_XYZ;
927
928                                 if ((!is_xyz && curves.size() != 1) || (is_xyz && curves.size() != 3)) {
929                                         if (is_xyz)
930                                                 fprintf(stderr, "%s: expected 3 curves, got %d\n", path, (int)curves.size());
931                                         else
932                                                 fprintf(stderr, "%s: expected 1 curve, got %d\n", path, (int)curves.size());
933                                         return false;
934                                 }
935                                 
936                                 switch (animclass) {
937                                 case COLLADAFW::AnimationList::POSITION_X:
938                                         vec[0] = evaluate_fcurve(curves[0], fra);
939                                         break;
940                                 case COLLADAFW::AnimationList::POSITION_Y:
941                                         vec[1] = evaluate_fcurve(curves[0], fra);
942                                         break;
943                                 case COLLADAFW::AnimationList::POSITION_Z:
944                                         vec[2] = evaluate_fcurve(curves[0], fra);
945                                         break;
946                                 case COLLADAFW::AnimationList::POSITION_XYZ:
947                                         vec[0] = evaluate_fcurve(curves[0], fra);
948                                         vec[1] = evaluate_fcurve(curves[1], fra);
949                                         vec[2] = evaluate_fcurve(curves[2], fra);
950                                         break;
951                                 default:
952                                         fprintf(stderr, "%s: animation class %d is not supported yet\n", path, animclass);
953                                         break;
954                                 }
955                         }
956                         else if (type == COLLADAFW::Transformation::MATRIX) {
957                                 // for now, of matrix animation, support only the case when all values are packed into one animation
958                                 if (curves.size() != 16) {
959                                         fprintf(stderr, "%s: expected 16 curves, got %d\n", path, (int)curves.size());
960                                         return false;
961                                 }
962
963                                 COLLADABU::Math::Matrix4 matrix;
964                                 int i = 0, j = 0;
965
966                                 for (std::vector<FCurve*>::iterator it = curves.begin(); it != curves.end(); it++) {
967                                         matrix.setElement(i, j, evaluate_fcurve(*it, fra));
968                                         j++;
969                                         if (j == 4) {
970                                                 i++;
971                                                 j = 0;
972                                         }
973                                 }
974
975                                 COLLADAFW::Matrix tm(matrix);
976                                 dae_matrix_to_mat4(&tm, mat);
977
978                                 return true;
979                         }
980                 }
981
982                 if (is_scale)
983                         size_to_mat4(mat, vec);
984                 else
985                         copy_v3_v3(mat[3], vec);
986
987                 return is_scale || is_translate;
988         }
989
990         return false;
991 }
992
993 // gives a world-space mat of joint at rest position
994 void AnimationImporter::get_joint_rest_mat(float mat[4][4], COLLADAFW::Node *root, COLLADAFW::Node *node)
995 {
996         // if bind mat is not available,
997         // use "current" node transform, i.e. all those tms listed inside <node>
998         if (!armature_importer->get_joint_bind_mat(mat, node)) {
999                 float par[4][4], m[4][4];
1000
1001                 calc_joint_parent_mat_rest(par, NULL, root, node);
1002                 get_node_mat(m, node, NULL, NULL);
1003                 mul_m4_m4m4(mat, m, par);
1004         }
1005 }
1006
1007 // gives a world-space mat, end's mat not included
1008 bool AnimationImporter::calc_joint_parent_mat_rest(float mat[4][4], float par[4][4], COLLADAFW::Node *node, COLLADAFW::Node *end)
1009 {
1010         float m[4][4];
1011
1012         if (node == end) {
1013                 par ? copy_m4_m4(mat, par) : unit_m4(mat);
1014                 return true;
1015         }
1016
1017         // use bind matrix if available or calc "current" world mat
1018         if (!armature_importer->get_joint_bind_mat(m, node)) {
1019                 if (par) {
1020                         float temp[4][4];
1021                         get_node_mat(temp, node, NULL, NULL);
1022                         mul_m4_m4m4(m, temp, par);
1023                 }
1024                 else {
1025                         get_node_mat(m, node, NULL, NULL);
1026                 }
1027         }
1028
1029         COLLADAFW::NodePointerArray& children = node->getChildNodes();
1030         for (unsigned int i = 0; i < children.getCount(); i++) {
1031                 if (calc_joint_parent_mat_rest(mat, m, children[i], end))
1032                         return true;
1033         }
1034
1035         return false;
1036 }
1037
1038 #ifdef ARMATURE_TEST
1039 Object *AnimationImporter::get_joint_object(COLLADAFW::Node *root, COLLADAFW::Node *node, Object *par_job)
1040 {
1041         if (joint_objects.find(node->getUniqueId()) == joint_objects.end()) {
1042                 Object *job = add_object(scene, OB_EMPTY);
1043
1044                 rename_id((ID*)&job->id, (char*)get_joint_name(node));
1045
1046                 job->lay = object_in_scene(job, scene)->lay = 2;
1047
1048                 mul_v3_fl(job->size, 0.5f);
1049                 job->recalc |= OB_RECALC_OB;
1050
1051                 verify_adt_action((ID*)&job->id, 1);
1052
1053                 job->rotmode = ROT_MODE_QUAT;
1054
1055                 float mat[4][4];
1056                 get_joint_rest_mat(mat, root, node);
1057
1058                 if (par_job) {
1059                         float temp[4][4], ipar[4][4];
1060                         invert_m4_m4(ipar, par_job->obmat);
1061                         copy_m4_m4(temp, mat);
1062                         mul_m4_m4m4(mat, temp, ipar);
1063                 }
1064
1065                 TransformBase::decompose(mat, job->loc, NULL, job->quat, job->size);
1066
1067                 if (par_job) {
1068                         job->parent = par_job;
1069
1070                         par_job->recalc |= OB_RECALC_OB;
1071                         job->parsubstr[0] = 0;
1072                 }
1073
1074                 where_is_object(scene, job);
1075
1076                 // after parenting and layer change
1077                 DAG_scene_sort(CTX_data_main(C), scene);
1078
1079                 joint_objects[node->getUniqueId()] = job;
1080         }
1081
1082         return joint_objects[node->getUniqueId()];
1083 }
1084 #endif
1085
1086 #if 0
1087 // recursively evaluates joint tree until end is found, mat then is world-space matrix of end
1088 // mat must be identity on enter, node must be root
1089 bool AnimationImporter::evaluate_joint_world_transform_at_frame(float mat[4][4], float par[4][4], COLLADAFW::Node *node, COLLADAFW::Node *end, float fra)
1090 {
1091         float m[4][4];
1092         if (par) {
1093                 float temp[4][4];
1094                 evaluate_transform_at_frame(temp, node, node == end ? fra : 0.0f);
1095                 mul_m4_m4m4(m, temp, par);
1096         }
1097         else {
1098                 evaluate_transform_at_frame(m, node, node == end ? fra : 0.0f);
1099         }
1100
1101         if (node == end) {
1102                 copy_m4_m4(mat, m);
1103                 return true;
1104         }
1105         else {
1106                 COLLADAFW::NodePointerArray& children = node->getChildNodes();
1107                 for (int i = 0; i < children.getCount(); i++) {
1108                         if (evaluate_joint_world_transform_at_frame(mat, m, children[i], end, fra))
1109                                 return true;
1110                 }
1111         }
1112
1113         return false;
1114 }
1115 #endif
1116
1117 void AnimationImporter::add_bone_fcurve(Object *ob, COLLADAFW::Node *node, FCurve *fcu)
1118 {
1119         const char *bone_name = bc_get_joint_name(node);
1120         bAction *act = ob->adt->action;
1121                         
1122         /* try to find group */
1123         bActionGroup *grp = action_groups_find_named(act, bone_name);
1124
1125         /* no matching groups, so add one */
1126         if (grp == NULL) {
1127                 /* Add a new group, and make it active */
1128                 grp = (bActionGroup*)MEM_callocN(sizeof(bActionGroup), "bActionGroup");
1129                                         
1130                 grp->flag = AGRP_SELECTED;
1131                 BLI_strncpy(grp->name, bone_name, sizeof(grp->name));
1132                                         
1133                 BLI_addtail(&act->groups, grp);
1134                 BLI_uniquename(&act->groups, grp, "Group", '.', offsetof(bActionGroup, name), 64);
1135         }
1136                                 
1137         /* add F-Curve to group */
1138         action_groups_add_channel(act, grp, fcu);
1139 }
1140
1141 void AnimationImporter::add_bezt(FCurve *fcu, float fra, float value)
1142 {
1143         BezTriple bez;
1144         memset(&bez, 0, sizeof(BezTriple));
1145         bez.vec[1][0] = fra;
1146         bez.vec[1][1] = value;
1147         bez.ipo = U.ipo_new; /* use default interpolation mode here... */
1148         bez.f1 = bez.f2 = bez.f3 = SELECT;
1149         bez.h1 = bez.h2 = HD_AUTO;
1150         insert_bezt_fcurve(fcu, &bez, 0);
1151         calchandles_fcurve(fcu);
1152 }