00001
00002
00003
00004
00005
00006
00007
00008
00009
00010
00011
00012
00013
00014
00015
00016
00017
00018
00019
00020
00021
00022
00023
00024
00025
00026
00027
00028
00029
00030
00031
00032
00033
00034
00035 #include "bvh.h"
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
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
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
00087 { NULL, eIndex_Invalid }
00088 };
00089
00090
00091
00092
00093
00094
00095
00096
00097
00098
00099
00100
00101
00102
00103
00104
00105
00106
00107
00108
00109
00110
00111
00112
00113
00114
00115
00116
00117
00118
00119
00120
00121
00122
00123
00124
00125
00126
00127
00128
00129
00130
00131
00132
00133
00134
00135
00136
00137
00138
00139
00140
00141
00142
00143
00144
00145
00146
00147
00148
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
00260
00261
00262
00263 class Mocap : public Skeleton {
00264 public:
00265
00266 Mocap(void) throw() { }
00267 ~Mocap(void) throw() { }
00268
00269
00270 void initialize(IO std::istream& stream);
00271
00272
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
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
00294 std::string name;
00295 point3d_t pos;
00296 int nChannels;
00297
00298
00299 eChannelIndex operation[eIndex_Max];
00300
00301
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
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
00322 vec_node_t m_roots;
00323 int m_nChannels;
00324 int m_nFrames;
00325 float m_frameTime;
00326 std::vector<float> m_animData;
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
00343
00344
00345 expectToken(stream, "HIERARCHY");
00346
00347
00348 m_nChannels = 0;
00349 while (true) {
00350 line = getNextLineFromStream(stream, parseFlags);
00351
00352 if ("" == line) {
00353 continue;
00354 }
00355 const char * p = line.c_str();
00356 p = getNextTokenFromString(p, token, parseFlags);
00357
00358 if ("ROOT" != token) {
00359 break;
00360 }
00361
00362
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
00371
00372 ASSERT_THROW(m_roots.size(), "BVH stream contained no ROOT objects?");
00373
00374
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
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;
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
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
00442
00443 ASSERT(objIndex >= 0 && objIndex < (int) m_roots.size(),
00444 "Invalid object index: %d", objIndex);
00445 ASSERT(vertices, "null");
00446
00447
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
00458 ASSERT(nFrame >= 0 && nFrame < m_nFrames, "bad frame: %d", nFrame);
00459
00460
00461 const float * p = &m_animData[nFrame * m_nChannels];
00462 const Node * root = m_roots[objIndex];
00463 ASSERT(root, "null");
00464
00465
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
00506
00507
00508
00509 smart_ptr<Mocap::Node>
00510 Mocap::parseNode
00511 (
00512 IO std::istream& stream,
00513 IN const char * p
00514 )
00515 {
00516 ASSERT_THROW(stream.good(), "bad bvh stream while parsing node");
00517 ASSERT(p, "null");
00518
00519
00520 smart_ptr<Node> node = new Node;
00521 ASSERT(node, "out of memory");
00522
00523
00524 eParseBehavior parseFlags = eParse_Strip;
00525 p = getNextTokenFromString(p, node->name, parseFlags);
00526 expectToken(stream, "{");
00527
00528
00529 std::string line, token;
00530 while (true) {
00531 line = getNextLineFromStream(stream, parseFlags);
00532 if ("" == line)
00533 continue;
00534 const char * p = line.c_str();
00535 p = getNextTokenFromString(p, token, parseFlags);
00536
00537 if ("OFFSET" == token) {
00538
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
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
00555 break;
00556 } else {
00557 ASSERT_THROW(false, "bad keyword in node: " << token);
00558 }
00559 }
00560
00561
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;
00578
00579
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
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
00636
00637
00638 pos->parent = parent;
00639
00640
00641 strncpy(pos->name, node->name.c_str(), node_position_t::eMaxNameSize);
00642 pos->name[node_position_t::eMaxNameSize - 1] = 0;
00643
00644
00645
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;
00651 int idx = node->valueIndex[i];
00652
00653 doTransformation(M, node->operation[i], values[idx]);
00654 }
00655
00656
00657
00658
00659
00660 M += node->pos;
00661
00662
00663 M = parentT * M;
00664
00665
00666
00667 pos->pos = M.getTranslation();
00668
00669
00670 pos->T = M;
00671
00672
00673 node_position_t * self = pos;
00674 ++pos;
00675
00676
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
00686 return pos;
00687 }
00688
00689
00690
00691
00692
00693
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 };
00727