1 files changed, 157 insertions, 0 deletions
diff --git a/dev/a4-dance/motion_clip.cc b/dev/a4-dance/motion_clip.cc
new file mode 100644
index 0000000..f239aec
--- /dev/null
+++ b/dev/a4-dance/motion_clip.cc
@@ -0,0 +1,157 @@
+#include "motion_clip.h"
+
+#include <fstream>
+
+#include "amc_util.h"
+#include "simple_parser.h"
+
+MotionClip::MotionClip() {
+}
+
+MotionClip::~MotionClip() {
+}
+
+int MotionClip::size() const {
+    return (int)frames_.size();
+}
+
+Pose MotionClip::pose(int frame_num) const {
+    return frames_[frame_num];
+}
+
+Pose& MotionClip::pose(int frame_num) {
+    return frames_[frame_num];
+}
+
+void MotionClip::TrimFront(int num_frames) {
+    frames_.erase(frames_.begin(), frames_.begin() + num_frames);
+}
+
+void MotionClip::TrimBack(int num_frames) {
+    frames_.erase(frames_.end()-num_frames-1, frames_.end()-1);
+}
+
+void MotionClip::PrependPose(const Pose &pose) {
+    frames_.insert(frames_.begin(), pose);
+}
+
+void MotionClip::AppendPose(const Pose &pose) {
+    frames_.push_back(pose);
+}
+
+
+void MotionClip::PrependClip(const MotionClip &clip, int num_blend_frames) {
+    for (int i=0; i < clip.size()-num_blend_frames; i++) {
+        frames_.insert(frames_.begin(), clip[i]);
+    }
+    for (int i=0; i<num_blend_frames; i++) {
+        int clip_index = clip.size() - num_blend_frames + i;
+        int orig_index = clip_index;
+        float alpha = (float)i/(float)(num_blend_frames-1);  // 0.0 to 1.0
+        Pose blended = clip[clip_index].Lerp(frames_[orig_index], alpha);
+        frames_[orig_index] = blended;
+    }
+}
+
+void MotionClip::AppendClip(const MotionClip &clip, int num_blend_frames) {
+    for (int i=0; i<num_blend_frames; i++) {
+        int clip_index = i;
+        int orig_index = (int)frames_.size() - num_blend_frames + i;
+        float alpha = (float)i/(float)(num_blend_frames-1);  // 0.0 to 1.0
+        Pose blended = frames_[orig_index].Lerp(clip[clip_index], alpha);
+        frames_[orig_index] = blended;
+    }
+    for (int i=num_blend_frames; i < clip.size(); i++) {
+        frames_.push_back(clip[i]);
+    }
+}
+
+
+void MotionClip::MakeLoop(int num_blend_frames) {
+    MotionClip tmp_clip;
+    for (int i=0; i<num_blend_frames; i++) {
+        tmp_clip.PrependPose(frames_.back());
+        frames_.pop_back();
+    }
+    for (int i=0; i<tmp_clip.size(); i++) {
+        float alpha = (float)i/(float)(tmp_clip.size()-1);
+        frames_[i] = tmp_clip[i].Lerp(frames_[i], alpha);
+    }
+}
+
+void MotionClip::CalcRelativeTranslations() {
+    Point3 prev_pos = frames_[0].root_position();
+    for (int i=0; i<frames_.size(); i++) {
+        Point3 cur_pos = frames_[i].root_position();
+        frames_[i].set_root_relative_translation(cur_pos - prev_pos);
+        prev_pos = cur_pos;
+    }
+}
+
+
+Pose MotionClip::operator[](const int frame_num) const {
+    return frames_[frame_num];
+}
+
+Pose& MotionClip::operator[](const int frame_num) {
+    return frames_[frame_num];
+}
+
+
+void MotionClip::LoadFromAMC(const std::string &filename, const Skeleton &skeleton) {
+    std::ifstream ifs(filename.c_str());
+    SimpleParser parser(&ifs);
+    
+    parser.SwallowLine();
+    parser.SwallowLine();
+    parser.SwallowLine();
+    
+    while (parser.Good()) {
+        Pose frame;
+        int frame_num;
+        parser.ReadInt(frame_num);
+        if (!parser.Good()) {
+            return;
+        }
+        frame.set_frame_number(frame_num);
+        
+        while (!parser.UpcomingInt()) {
+            std::string bone;
+            parser.ReadToken(bone);
+            if (!parser.Good()) {
+                return;
+            }
+            if (bone == "root") {
+                Point3 position;
+                parser.ReadFloat(position[0]);
+                parser.ReadFloat(position[1]);
+                parser.ReadFloat(position[2]);
+                position = amc2meter(position);
+                frame.set_root_position(position);
+                
+                Vector3 orientation;
+                parser.ReadFloat(orientation[0]);
+                parser.ReadFloat(orientation[1]);
+                parser.ReadFloat(orientation[2]);
+                frame.set_root_orientation(orientation);                
+            }
+            else {
+                float rx=0, ry=0, rz=0;
+                
+                if (skeleton.rx_dof(bone)) {
+                    parser.ReadFloat(rx);
+                }
+                if (skeleton.ry_dof(bone)) {
+                    parser.ReadFloat(ry);
+                }
+                if (skeleton.rz_dof(bone)) {
+                    parser.ReadFloat(rz);
+                }
+                frame.set_joint_angles(bone, Vector3(rx, ry, rz));
+            }
+        }
+        frames_.push_back(frame);
+    }
+}
+
+