bvh.cpp

Go to the documentation of this file.
00001 /*
00002  * bvh.cpp
00003  *
00004  * Copyright (C) 2009  Thomas A. Vaughan
00005  * All rights reserved.
00006  *
00007  *
00008  * Redistribution and use in source and binary forms, with or without
00009  * modification, are permitted provided that the following conditions are met:
00010  *     * Redistributions of source code must retain the above copyright
00011  *       notice, this list of conditions and the following disclaimer.
00012  *     * Redistributions in binary form must reproduce the above copyright
00013  *       notice, this list of conditions and the following disclaimer in the
00014  *       documentation and/or other materials provided with the distribution.
00015  *     * Neither the name of the <organization> nor the
00016  *       names of its contributors may be used to endorse or promote products
00017  *       derived from this software without specific prior written permission.
00018  *
00019  * THIS SOFTWARE IS PROVIDED BY THOMAS A. VAUGHAN ''AS IS'' AND ANY
00020  * EXPRESS OR IMPLIED WARRANTIES, INCLUDING, BUT NOT LIMITED TO, THE IMPLIED
00021  * WARRANTIES OF MERCHANTABILITY AND FITNESS FOR A PARTICULAR PURPOSE ARE
00022  * DISCLAIMED. IN NO EVENT SHALL THOMAS A. VAUGHAN BE LIABLE FOR ANY
00023  * DIRECT, INDIRECT, INCIDENTAL, SPECIAL, EXEMPLARY, OR CONSEQUENTIAL DAMAGES
00024  * (INCLUDING, BUT NOT LIMITED TO, PROCUREMENT OF SUBSTITUTE GOODS OR SERVICES;
00025  * LOSS OF USE, DATA, OR PROFITS; OR BUSINESS INTERRUPTION) HOWEVER CAUSED AND
00026  * ON ANY THEORY OF LIABILITY, WHETHER IN CONTRACT, STRICT LIABILITY, OR TORT
00027  * (INCLUDING NEGLIGENCE OR OTHERWISE) ARISING IN ANY WAY OUT OF THE USE OF THIS
00028  * SOFTWARE, EVEN IF ADVISED OF THE POSSIBILITY OF SUCH DAMAGE.
00029  *
00030  *
00031  * Parsing of BVH files.  See bvh.h
00032  */
00033 
00034 // includes --------------------------------------------------------------------
00035 #include "bvh.h"                // always include our own header first
00036 
00037 #include <fstream>
00038 #include <math.h>
00039 
00040 #include "common/wave_ex.h"
00041 #include "geometry/matrix_4.h"
00042 #include "perf/perf.h"
00043 #include "util/parsing.h"
00044 #include "util/token_stream.h"
00045 
00046 
00047 namespace bvh {
00048 
00049 
00050 // interface destructors
00051 Skeleton::~Skeleton(void) throw() { }
00052 
00053 
00054 static const float s_radiansPerDegree           = M_PI / 180.0;
00055 
00056 
00057 enum eChannelIndex {
00058         eIndex_Xposition        = 0,
00059         eIndex_Yposition        = 1,
00060         eIndex_Zposition        = 2,
00061         eIndex_Xrotation        = 3,
00062         eIndex_Yrotation        = 4,
00063         eIndex_Zrotation        = 5,
00064 
00065         // keep these last!
00066         eIndex_Max              = 6,
00067         eIndex_Invalid          = -1
00068 };
00069 
00070 struct index_entry_t {
00071         const char *            name;
00072         eChannelIndex           index;
00073 };
00074 
00075 #define ENTRY( val ) { #val , eIndex_ ##val },
00076 
00077 
00078 static const index_entry_t s_indexTable[] = {
00079         ENTRY(Zrotation)
00080         ENTRY(Xrotation)
00081         ENTRY(Yrotation)
00082         ENTRY(Xposition)
00083         ENTRY(Yposition)
00084         ENTRY(Zposition)
00085 
00086         // keep this last
00087         { NULL, eIndex_Invalid }
00088 };
00089 
00090 
00091 ////////////////////////////////////////////////////////////////////////////////
00092 ///
00093 /// \ingroup bvh
00094 /// \defgroup bvh_file_format BVH File Format and Transformations
00095 ///
00096 /// This is a quick description of the BVH file format and how transformations
00097 /// work.
00098 ///
00099 /// The format itself is pretty basic (see
00100 /// http://www.google.com/search?q=bvh+file+format for examples)
00101 /// but getting the transformations correct is a bit tedious.
00102 ///
00103 /// The BVH format consists of nodes (ROOT, JOINT, or End Site objects), each
00104 /// with a position relative to their parent.  Each node also defines how to
00105 /// extract data from each animation frame relevant to that node.  Typically
00106 /// that is rotation information.
00107 ///
00108 /// Suppose two joint objects are defined as such:
00109 /// \code
00110 ///    JOINT Elbow
00111 ///    {
00112 ///        OFFSET   2.5 0 0
00113 ///        CHANNELS 3 Zrotation Xrotation Yrotation
00114 ///        JOINT Hand {
00115 ///            OFFSET   0 3.5 0
00116 ///            CHANNELS Zrotation Xrotation Yrotation
00117 ///        }
00118 ///    }
00119 /// \endcode
00120 ///
00121 /// Here we've got an Elbow joint with a child Hand joint.  Furthermore, each
00122 /// has 3 angles of rotation, and all 3 rotation angles will be specified in
00123 /// each frame of animation.
00124 ///
00125 /// With no rotations, the Hand is located 3.5 units from the elbow, in the Y
00126 /// direction.  And the Elbow is located 2.5 units from its parent, in the X
00127 /// direction.  (these positions are from the OFFSET element)
00128 ///
00129 /// Let's suppose the Elbow was the root object, at the origin.  What is the
00130 /// location of the Hand?  It is at X=2.5, Y=3.5, of course.
00131 ///
00132 /// But now suppose there are rotations in the animation frames.  Now more
00133 /// complicated matrices have to be calculated for each step.  It roughly
00134 /// works like this for each node:
00135 ///
00136 ///  - start with the parent node's transformation
00137 ///  - apply the child OFFSET
00138 ///  - apply the child rotation
00139 ///
00140 /// So in particular, rotations of a given child node don't affect its own
00141 /// translation.  Rotations of a given child node only impact its
00142 /// descendants.
00143 ///
00144 ////////////////////////////////////////////////////////////////////////////////
00145 
00146 ////////////////////////////////////////////////////////////////////////////////
00147 //
00148 //      static helper methods
00149 //
00150 ////////////////////////////////////////////////////////////////////////////////
00151 
00152 static void
00153 doTransformation
00154 (
00155 IO matrix4_t& M,
00156 IN eChannelIndex i,
00157 IN float x
00158 )
00159 throw()
00160 {
00161         ASSERT(i >= 0 && i < eIndex_Max, "Bad channel index: %d", i);
00162 
00163         switch (i) {
00164         case eIndex_Xposition:
00165                 M.m[3] += x;
00166                 break;
00167 
00168         case eIndex_Yposition:
00169                 M.m[7] += x;
00170                 break;
00171 
00172         case eIndex_Zposition:
00173                 M.m[11] += x;
00174                 break;
00175 
00176         case eIndex_Xrotation:
00177                 {
00178                         matrix4_t A;
00179                         A.setXRotation(-x * s_radiansPerDegree);
00180                         M = M * A;
00181                 }
00182                 break;
00183 
00184         case eIndex_Yrotation:
00185                 {
00186                         matrix4_t A;
00187                         A.setYRotation(-x * s_radiansPerDegree);
00188                         M = M * A;
00189                 }
00190                 break;
00191 
00192         case eIndex_Zrotation:
00193                 {
00194                         matrix4_t A;
00195                         A.setZRotation(-x * s_radiansPerDegree);
00196                         M = M * A;
00197                 }
00198                 break;
00199 
00200         default:
00201                 ASSERT(false, "should never get here!");
00202         }
00203 }
00204 
00205 
00206 
00207 static eChannelIndex
00208 getChannelIndex
00209 (
00210 IN const char * name
00211 )
00212 {
00213         ASSERT(name, "null");
00214 
00215         for (const index_entry_t * p = s_indexTable; p->name; ++p) {
00216                 if (!strcmp(p->name, name)) {
00217                         return p->index;
00218                 }
00219         }
00220 
00221         ASSERT_THROW(false, "Channel index not found: " << name);
00222         return eIndex_Invalid;
00223 }
00224 
00225 
00226 
00227 static const char *
00228 readFloat
00229 (
00230 IN const char * p,
00231 IO std::string& token,
00232 OUT float& f
00233 )
00234 {
00235         p = getNextTokenFromString(p, token, eParse_Strip);
00236         f = atof(token.c_str());
00237         return p;
00238 }
00239 
00240 
00241 
00242 static const char *
00243 readInt
00244 (
00245 IN const char * p,
00246 IO std::string& token,
00247 OUT int& i
00248 )
00249 {
00250         p = getNextTokenFromString(p, token, eParse_Strip);
00251         i = atoi(token.c_str());
00252         return p;
00253 }
00254 
00255 
00256 
00257 ////////////////////////////////////////////////////////////////////////////////
00258 //
00259 //      Mocap -- class that implements the bvh::Skeleton interface
00260 //
00261 ////////////////////////////////////////////////////////////////////////////////
00262 
00263 class Mocap : public Skeleton {
00264 public:
00265         // constructor, destructor ---------------------------------------------
00266         Mocap(void) throw() { }
00267         ~Mocap(void) throw() { }
00268 
00269         // public class methods ------------------------------------------------
00270         void initialize(IO std::istream& stream);
00271 
00272         // bvh::Skeleton class interface methods -------------------------------
00273         int getObjectCount(void) const throw() { return m_roots.size(); }
00274         int getNodeCount(IN int objIndex) const throw();
00275         void getNodePositions(IN int objIndex,
00276                                 IN float time,
00277                                 OUT node_position_t * p) const throw();
00278         void dump(IN const char * title) const throw();
00279 
00280 private:
00281         // private data structures ---------------------------------------------
00282         struct Node {
00283                 Node(void) throw() { this->clear(); }
00284                 void clear(void) throw() {
00285                                 pos.clear();
00286                                 nChannels = 0;
00287                                 for (int i = 0; i < eIndex_Max; ++i) {
00288                                         operation[i] = eIndex_Invalid;
00289                                         valueIndex[i] = -1;
00290                                 }
00291                         }
00292 
00293                 // data fields
00294                 std::string             name;
00295                 point3d_t               pos;
00296                 int                     nChannels;
00297 
00298                 // operation[] - ordered list of operations. 
00299                 eChannelIndex           operation[eIndex_Max];
00300 
00301                 // valueIndex[] - for operation, indexes value
00302                 int                     valueIndex[eIndex_Max];
00303 
00304                 std::vector<smart_ptr<Node> > children;
00305         };
00306 
00307         typedef std::vector<smart_ptr<Node> > vec_node_t;
00308 
00309         // private helper methods ----------------------------------------------
00310         smart_ptr<Node> parseNode(IO std::istream& stream,
00311                                 IN const char * p);
00312         int getNodeCount(IN const Node * node) const throw();
00313         void dumpNode(IN const char * indent,
00314                                 IN const Node * node) const throw();
00315         node_position_t * setNodePositions(IN const Node * node,
00316                                 IN const matrix4_t& parentT,
00317                                 IN const float * values,
00318                                 IN node_position_t * pos,
00319                                 IN node_position_t * parent) const throw();
00320 
00321         // private member data -------------------------------------------------
00322         vec_node_t              m_roots;        // root objects
00323         int                     m_nChannels;    // total animation channels
00324         int                     m_nFrames;      // number of animation frames
00325         float                   m_frameTime;    // seconds per frame
00326         std::vector<float>      m_animData;     // raw animation data
00327 };
00328 
00329 
00330 
00331 void
00332 Mocap::initialize
00333 (
00334 IO std::istream& stream
00335 )
00336 {
00337         ASSERT_THROW(stream.good(), "bad BVH data stream");
00338 
00339         eParseBehavior parseFlags = eParse_Strip;
00340         std::string line, token;
00341 
00342         // parse the bvh file.
00343         // See http://www.cs.wisc.edu/graphics/Courses/cs-838-1999/Jeff/BVH.html
00344         // first token should be "HIERARCHY"
00345         expectToken(stream, "HIERARCHY");
00346 
00347         // keep parsing root objects
00348         m_nChannels = 0;        // important to reset channel counter!
00349         while (true) {
00350                 line = getNextLineFromStream(stream, parseFlags);
00351         //      DPRINTF("line = '%s'", line.c_str());
00352                 if ("" == line) {
00353                         continue;               // skip empty lines
00354                 }
00355                 const char * p = line.c_str();
00356                 p = getNextTokenFromString(p, token, parseFlags);
00357         //      DPRINTF("token = '%s'", token.c_str());
00358                 if ("ROOT" != token) {
00359                         break;          // not a root?  break out!
00360                 }
00361 
00362                 // parse node
00363                 try {
00364                         m_roots.push_back(this->parseNode(stream, p));
00365                 } catch (std::exception& e) {
00366                         WAVE_EX(wex);
00367                         wex << "Failed to parse bvh node: " << e.what();
00368                 }
00369         }
00370         //DPRINTF("Parsed %d root objects", m_roots.size());
00371         //DPRINTF("Require %d animation channels", m_nChannels);
00372         ASSERT_THROW(m_roots.size(), "BVH stream contained no ROOT objects?");
00373 
00374         // now parse motion
00375         ASSERT_THROW("MOTION" == token, "Invalid token in BVH stream");
00376         expectToken(stream, "Frames:");
00377         getNextToken(stream, token);
00378         m_nFrames = atoi(token.c_str());
00379         expectToken(stream, "Frame");
00380         expectToken(stream, "Time:");
00381         getNextToken(stream, token);
00382         m_frameTime = atof(token.c_str());
00383 
00384         // allocate a single large array for all animation data
00385         long nFloats = m_nChannels * m_nFrames;
00386         m_animData.reserve(nFloats);
00387 
00388         float * pf = &m_animData[0];
00389         for (int i = 0; i < m_nFrames; ++i) {
00390                 line = getNextLineFromStream(stream, parseFlags);
00391                 if ("" == line) {
00392                         --i;
00393                         continue;       // skip empty lines
00394                 }
00395                 const char * p = line.c_str();
00396                 for (int j = 0; j < m_nChannels; ++j, ++pf) {
00397                         p = readFloat(p, token, *pf);
00398                 }
00399         }
00400 }
00401 
00402 
00403 
00404 ////////////////////////////////////////////////////////////////////////////////
00405 //
00406 //      Mocap -- bvh::Skeleton class interface methods
00407 //
00408 ////////////////////////////////////////////////////////////////////////////////
00409 
00410 int
00411 Mocap::getNodeCount
00412 (
00413 IN int objIndex
00414 )
00415 const
00416 throw()
00417 {
00418         ASSERT(objIndex >= 0 && objIndex < (int) m_roots.size(),
00419             "Invalid object index: %d", objIndex);
00420 
00421         const Node * root = m_roots[objIndex];
00422         ASSERT(root, "null root node in array");
00423 
00424         return this->getNodeCount(root);
00425 }
00426 
00427 
00428 
00429 void
00430 Mocap::getNodePositions
00431 (
00432 IN int objIndex,
00433 IN float time,
00434 OUT node_position_t * vertices
00435 )
00436 const
00437 throw()
00438 {
00439         perf::Timer timer("Skeleton::getNodePositions");
00440 
00441         //DPRINTF("\n\nSETTING POSITIONS=================================");
00442 
00443         ASSERT(objIndex >= 0 && objIndex < (int) m_roots.size(),
00444             "Invalid object index: %d", objIndex);
00445         ASSERT(vertices, "null");
00446 
00447         // get animation frame
00448         ASSERT(m_frameTime > 0, "bad animation frame time: %f", m_frameTime);
00449         int nFrame = (int) ((time / m_frameTime) + 0.5);
00450         if (nFrame < 0) {
00451                 int offset = -nFrame;
00452                 int nAnimations = (int)(offset / m_nFrames);
00453                 nAnimations += 1;
00454                 nFrame += m_nFrames * nAnimations;
00455         }
00456         nFrame = nFrame % m_nFrames;
00457         //DPRINTF("Time = %f seconds  ==>  frame %d", time, nFrame);
00458         ASSERT(nFrame >= 0 && nFrame < m_nFrames, "bad frame: %d", nFrame);
00459 
00460         // find this frame
00461         const float * p = &m_animData[nFrame * m_nChannels];
00462         const Node * root = m_roots[objIndex];
00463         ASSERT(root, "null");
00464 
00465         // walk the node tree and populate the node positions
00466         matrix4_t M;
00467         M.setIdentity();
00468         this->setNodePositions(root, M, p, vertices, NULL);
00469 }
00470 
00471 
00472 
00473 void
00474 Mocap::dump
00475 (
00476 IN const char * title
00477 )
00478 const
00479 throw()
00480 {
00481         ASSERT(title, "null");
00482 
00483         DPRINTF("%s bvh skeleton:", title);
00484         DPRINTF("  contains %d root nodes", (int) m_roots.size());
00485         DPRINTF("  there are %d animation channels", m_nChannels);
00486         DPRINTF("  contains %d animation frames", m_nFrames);
00487         DPRINTF("  each frame is %f seconds", m_frameTime);
00488 
00489         int N = this->getObjectCount();
00490         DPRINTF("  Got %d objects", N);
00491         for (int i = 0; i < N; ++i) {
00492                 DPRINTF("    object %d contains %d nodes (vertices)",
00493                     i, this->getNodeCount(i));
00494         }
00495 
00496         for (int i = 0; i < N; ++i) {
00497                 this->dumpNode("  ", m_roots[i]);
00498         }
00499 }
00500 
00501 
00502 
00503 ////////////////////////////////////////////////////////////////////////////////
00504 //
00505 //      Mocap -- private helper methods
00506 //
00507 ////////////////////////////////////////////////////////////////////////////////
00508 
00509 smart_ptr<Mocap::Node>
00510 Mocap::parseNode
00511 (
00512 IO std::istream& stream,
00513 IN const char * p               ///< remainder of line
00514 )
00515 {
00516         ASSERT_THROW(stream.good(), "bad bvh stream while parsing node");
00517         ASSERT(p, "null");
00518 
00519         // allocate memory for new node
00520         smart_ptr<Node> node = new Node;
00521         ASSERT(node, "out of memory");
00522 
00523         // read name and opening bracket
00524         eParseBehavior parseFlags = eParse_Strip;
00525         p = getNextTokenFromString(p, node->name, parseFlags);
00526         expectToken(stream, "{");
00527 
00528         // keep parsing lines
00529         std::string line, token;
00530         while (true) {
00531                 line = getNextLineFromStream(stream, parseFlags);
00532                 if ("" == line)
00533                         continue;       // skip empty lines
00534                 const char * p = line.c_str();
00535                 p = getNextTokenFromString(p, token, parseFlags);
00536 
00537                 if ("OFFSET" == token) {
00538                         // read x y z offset
00539                         p = readFloat(p, token, node->pos.x);
00540                         p = readFloat(p, token, node->pos.y);
00541                         p = readFloat(p, token, node->pos.z);
00542                         // node->pos.dump("offset");
00543                 } else if ("CHANNELS" == token) {
00544                         p = readInt(p, token, node->nChannels);
00545                         for (int i = 0; i < node->nChannels; ++i) {
00546                                 p = getNextTokenFromString(p, token, parseFlags);
00547                                 node->operation[i] = getChannelIndex(token.c_str());
00548                                 node->valueIndex[i] = m_nChannels;
00549                                 m_nChannels++;
00550                         }       
00551                 } else if ("JOINT" == token || "End" == token) {
00552                         node->children.push_back(this->parseNode(stream, p));
00553                 } else if ("}" == token) {
00554                         // end of node!
00555                         break;
00556                 } else {
00557                         ASSERT_THROW(false, "bad keyword in node: " << token);
00558                 }
00559         }
00560 
00561         // all done!
00562         return node;
00563 }
00564 
00565 
00566 
00567 int
00568 Mocap::getNodeCount
00569 (
00570 IN const Node * node
00571 )
00572 const
00573 throw()
00574 {
00575         ASSERT(node, "null");
00576 
00577         int count = 1;  // count this node
00578 
00579         // add children
00580         for (vec_node_t::const_iterator i = node->children.begin();
00581              i != node->children.end(); ++i) {
00582                 const Node * child = *i;
00583                 ASSERT(child, "null");
00584 
00585                 count += this->getNodeCount(child);
00586         }
00587 
00588         // all done
00589         return count;
00590 }
00591 
00592 
00593 
00594 void
00595 Mocap::dumpNode
00596 (
00597 IN const char * indent,
00598 IN const Node * node
00599 )
00600 const
00601 throw()
00602 {
00603         ASSERT(indent, "null");
00604         ASSERT(node, "null");
00605 
00606         DPRINTF("%sNode '%s'", indent, node->name.c_str());
00607         std::string newIndent = indent;
00608         newIndent += "  ";
00609         for (vec_node_t::const_iterator i = node->children.begin(); 
00610              i != node->children.end(); ++i) {
00611                 const Node * child = *i;
00612                 ASSERT(child, "null");
00613 
00614                 this->dumpNode(newIndent.c_str(), child);
00615         }
00616 }
00617 
00618 
00619 
00620 node_position_t *
00621 Mocap::setNodePositions
00622 (
00623 IN const Node * node,
00624 IN const matrix4_t& parentT,
00625 IN const float * values,
00626 IO node_position_t * pos,
00627 IN node_position_t * parent
00628 )
00629 const
00630 throw()
00631 {
00632         ASSERT(node, "null");
00633         ASSERT(values, "null");
00634         ASSERT(pos, "null");
00635         // ASSERT(parent) -- can be null!
00636 
00637         // copy parent
00638         pos->parent = parent;
00639 
00640         // copy name
00641         strncpy(pos->name, node->name.c_str(), node_position_t::eMaxNameSize);
00642         pos->name[node_position_t::eMaxNameSize - 1] = 0;       // force null
00643 
00644         // get local transformation
00645 //      parentT.dump("parent");
00646         matrix4_t M;
00647         M.setIdentity();
00648         for (int i = 0; i < eIndex_Max; ++i) {
00649                 if (eIndex_Invalid == node->operation[i])
00650                         break;          // end of operations!
00651                 int idx = node->valueIndex[i];
00652 //              DPRINTF("op: %d   idx: %d   val: %f", node->operation[i], idx, values[idx]);
00653                 doTransformation(M, node->operation[i], values[idx]);
00654         }
00655 //      M.dump("  After rotations");
00656 
00657         // add offset
00658 //      node->pos.dump("  child offset");
00659 //      M.dump(node->name.c_str());
00660         M += node->pos;
00661 //      M.dump("  After offset");
00662 
00663         M = parentT * M;
00664 //      M.dump("  total");
00665 
00666         // save this node's position
00667         pos->pos = M.getTranslation();
00668 
00669         // save transformation
00670         pos->T = M;
00671 
00672         // increment to next position
00673         node_position_t * self = pos;
00674         ++pos;
00675 
00676         // loop over children
00677         for (vec_node_t::const_iterator i = node->children.begin();
00678              i != node->children.end(); ++i) {
00679                 const Node * child = *i;
00680                 ASSERT(child, "null");
00681 
00682                 pos = this->setNodePositions(child, M, values, pos, self);
00683         }
00684 
00685         // all done!
00686         return pos;
00687 }
00688 
00689 
00690 
00691 ////////////////////////////////////////////////////////////////////////////////
00692 //
00693 //      public API
00694 //
00695 ////////////////////////////////////////////////////////////////////////////////
00696 
00697 smart_ptr<Skeleton>
00698 Skeleton::load
00699 (
00700 IN const char * filename
00701 )
00702 {
00703         perf::Timer timer("bvh::Skeleton::load");
00704         ASSERT(filename, "null");
00705 
00706         std::ifstream infile(filename);
00707         ASSERT_THROW(infile.good(), "Failed to open BVH file: " << filename);
00708 
00709         smart_ptr<Mocap> local = new Mocap;
00710         ASSERT(local, "out of memory");
00711 
00712         try {
00713                 local->initialize(infile);
00714         } catch (std::exception& e) {
00715                 WAVE_EX(wex);
00716                 wex << "Failed to parse skeleton animation from file: ";
00717                 wex << filename << "\n";
00718                 wex << "Failure: " << e.what();
00719         }
00720 
00721         return local;
00722 }
00723 
00724 
00725 
00726 };      // bvh namespace
00727