root/OpenSceneGraph/trunk/src/osgPlugins/bvh/ReaderWriterBVH.cpp @ 13041

Revision 13041, 14.7 kB (checked in by robert, 2 years ago)

Ran script to remove trailing spaces and tabs

  • Property svn:eol-style set to native
Line 
1#include <osg/Notify>
2#include <osg/Geometry>
3#include <osg/ShapeDrawable>
4
5#include <osgDB/FileNameUtils>
6#include <osgDB/FileUtils>
7#include <osgDB/Registry>
8
9#include <osgAnimation/Bone>
10#include <osgAnimation/Skeleton>
11#include <osgAnimation/UpdateBone>
12#include <osgAnimation/StackedTransform>
13#include <osgAnimation/StackedTranslateElement>
14#include <osgAnimation/StackedQuaternionElement>
15#include <osgAnimation/BasicAnimationManager>
16
17class BvhMotionBuilder : public osg::Referenced
18{
19public:
20    typedef std::pair<osg::ref_ptr<osgAnimation::Bone>, int> JointNode;
21    typedef std::vector<JointNode> JointList;
22
23    BvhMotionBuilder() : _drawingFlag(0) {}
24    virtual ~BvhMotionBuilder() {}
25
26    static BvhMotionBuilder* instance()
27    {
28        static osg::ref_ptr<BvhMotionBuilder> s_library = new BvhMotionBuilder;
29        return s_library.get();
30    }
31
32    void buildHierarchy( osgDB::Input& fr, int level, osgAnimation::Bone* parent )
33    {
34        bool isRecognized = false;
35        if ( !parent ) return;
36
37        if ( fr.matchSequence("OFFSET %f %f %f") )
38        {
39            isRecognized = true;
40            ++fr;
41
42            osg::Vec3 offset;
43            if ( fr.readSequence(offset) )
44            {
45                // Process OFFSET section
46                parent->setMatrixInSkeletonSpace( osg::Matrix::translate(offset) *
47                                                  parent->getMatrixInSkeletonSpace() );
48                osgAnimation::UpdateBone* updateBone =
49                    static_cast<osgAnimation::UpdateBone*>( parent->getUpdateCallback() );
50                if ( updateBone )
51                {
52                    osgAnimation::StackedTransform& stack = updateBone->getStackedTransforms();
53                    stack.push_back( new osgAnimation::StackedTranslateElement("position", offset) );
54                    stack.push_back( new osgAnimation::StackedQuaternionElement("quaternion", osg::Quat()) );
55                }
56
57                if ( _drawingFlag && parent->getNumParents() && level>0 )
58                    parent->getParent(0)->addChild( createRefGeometry(offset, 0.5).get() );
59            }
60        }
61
62        if ( fr.matchSequence("CHANNELS %i") )
63        {
64            isRecognized = true;
65
66            // Process CHANNELS section
67            int noChannels;
68            fr[1].getInt( noChannels );
69            fr += 2;
70
71            for ( int i=0; i<noChannels; ++i )
72            {
73                // Process each channel
74                std::string channelName;
75                fr.readSequence( channelName );
76                alterChannel( channelName, _joints.back().second );
77            }
78        }
79
80        if ( fr.matchSequence("End Site {") )
81        {
82            isRecognized = true;
83            fr += 3;
84
85            if ( fr.matchSequence("OFFSET %f %f %f") )
86            {
87                ++fr;
88
89                osg::Vec3 offsetEndSite;
90                if ( fr.readSequence(offsetEndSite) )
91                {
92                    // Process End Site section
93                    osg::ref_ptr<osgAnimation::Bone> bone = new osgAnimation::Bone( parent->getName()+"End" );
94                    bone->setMatrixInSkeletonSpace( osg::Matrix::translate(offsetEndSite) *
95                                                    bone->getMatrixInSkeletonSpace() );
96                    bone->setDataVariance( osg::Object::DYNAMIC );
97                    parent->insertChild( 0, bone.get() );
98
99                    if ( _drawingFlag )
100                        parent->addChild( createRefGeometry(offsetEndSite, 0.5).get() );
101                }
102            }
103            fr.advanceOverCurrentFieldOrBlock();
104        }
105
106        if ( fr.matchSequence("ROOT %w {") || fr.matchSequence("JOINT %w {") )
107        {
108            isRecognized = true;
109
110            // Process JOINT section
111            osg::ref_ptr<osgAnimation::Bone> bone = new osgAnimation::Bone( fr[1].getStr() );
112            bone->setDataVariance( osg::Object::DYNAMIC );
113            bone->setDefaultUpdateCallback();
114            parent->insertChild( 0, bone.get() );
115            _joints.push_back( JointNode(bone, 0) );
116
117            int entry = fr[1].getNoNestedBrackets();
118            fr += 3;
119            while ( !fr.eof() && fr[0].getNoNestedBrackets()>entry )
120                buildHierarchy( fr, entry, bone.get() );
121            fr.advanceOverCurrentFieldOrBlock();
122        }
123
124        if ( !isRecognized )
125        {
126            OSG_WARN << "BVH Reader: Unrecognized symbol " << fr[0].getStr()
127                << ". Ignore current field or block." << std::endl;
128            fr.advanceOverCurrentFieldOrBlock();
129        }
130    }
131
132    void buildMotion( osgDB::Input& fr, osgAnimation::Animation* anim )
133    {
134        int i=0, frames=0;
135        float frameTime=0.033f;
136
137        if ( !fr.readSequence("Frames:", frames) )
138        {
139            OSG_WARN << "BVH Reader: Frame number setting not found, but an unexpected "
140                << fr[0].getStr() << ". Set to 1." << std::endl;
141        }
142
143        ++fr;
144        if ( !fr.readSequence("Time:", frameTime) )
145        {
146            OSG_WARN << "BVH Reader: Frame time setting not found, but an unexpected "
147                << fr[0].getStr() << ". Set to 0.033 (30FPS)." << std::endl;
148        }
149
150        // Each joint has a position animating channel and an euler animating channel
151        std::vector< osg::ref_ptr<osgAnimation::Vec3LinearChannel> > posChannels;
152        std::vector< osg::ref_ptr<osgAnimation::QuatSphericalLinearChannel> > rotChannels;
153        for ( JointList::iterator itr=_joints.begin(); itr!=_joints.end(); ++itr )
154        {
155            std::string name = itr->first->getName();
156
157            osg::ref_ptr<osgAnimation::Vec3KeyframeContainer> posKey = new osgAnimation::Vec3KeyframeContainer;
158            osg::ref_ptr<osgAnimation::Vec3LinearSampler> posSampler = new osgAnimation::Vec3LinearSampler;
159            osg::ref_ptr<osgAnimation::Vec3LinearChannel> posChannel = new osgAnimation::Vec3LinearChannel( posSampler.get() );
160            posSampler->setKeyframeContainer( posKey.get() );
161            posChannel->setName( "position" );
162            posChannel->setTargetName( name );
163
164            osg::ref_ptr<osgAnimation::QuatKeyframeContainer> rotKey = new osgAnimation::QuatKeyframeContainer;
165            osg::ref_ptr<osgAnimation::QuatSphericalLinearSampler> rotSampler = new osgAnimation::QuatSphericalLinearSampler;
166            osg::ref_ptr<osgAnimation::QuatSphericalLinearChannel> rotChannel = new osgAnimation::QuatSphericalLinearChannel( rotSampler.get() );
167            rotSampler->setKeyframeContainer( rotKey.get() );
168            rotChannel->setName( "quaternion" );
169            rotChannel->setTargetName( name );
170
171            posChannels.push_back( posChannel );
172            rotChannels.push_back( rotChannel );
173        }
174
175        // Obtain motion data from the stream
176        while ( !fr.eof() && i<frames )
177        {
178            for ( unsigned int n=0; n<_joints.size(); ++n )
179            {
180                osgAnimation::Vec3LinearChannel* posChannel = posChannels[n].get();
181                osgAnimation::QuatSphericalLinearChannel* rotChannel = rotChannels[n].get();
182
183                setKeyframe( fr, _joints[n].second, frameTime*(double)i,
184                    dynamic_cast<osgAnimation::Vec3KeyframeContainer*>(posChannel->getSampler()->getKeyframeContainer()),
185                    dynamic_cast<osgAnimation::QuatKeyframeContainer*>(rotChannel->getSampler()->getKeyframeContainer()) );
186            }
187
188            i++;
189        }
190
191        // Add valid channels to the animate object
192        for ( unsigned int n=0; n<_joints.size(); ++n )
193        {
194            if ( posChannels[n]->getOrCreateSampler()->getOrCreateKeyframeContainer()->size()>0 )
195                anim->addChannel( posChannels[n].get() );
196            if ( rotChannels[n]->getOrCreateSampler()->getOrCreateKeyframeContainer()->size()>0 )
197                anim->addChannel( rotChannels[n].get() );
198        }
199    }
200
201    osg::Group* buildBVH( std::istream& stream, const osgDB::ReaderWriter::Options* options )
202    {
203        if ( options )
204        {
205            if ( options->getOptionString().find("contours")!=std::string::npos ) _drawingFlag = 1;
206            else if ( options->getOptionString().find("solids")!=std::string::npos ) _drawingFlag = 2;
207        }
208
209        osgDB::Input fr;
210        fr.attach( &stream );
211
212        osg::ref_ptr<osgAnimation::Bone> boneroot = new osgAnimation::Bone( "Root" );
213        boneroot->setDefaultUpdateCallback();
214
215        osg::ref_ptr<osgAnimation::Skeleton> skelroot = new osgAnimation::Skeleton;
216        skelroot->setDefaultUpdateCallback();
217        skelroot->insertChild( 0, boneroot.get() );
218
219        osg::ref_ptr<osgAnimation::Animation> anim = new osgAnimation::Animation;
220
221        while( !fr.eof() )
222        {
223            if ( fr.matchSequence("HIERARCHY") )
224            {
225                ++fr;
226                buildHierarchy( fr, 0, boneroot.get() );
227            }
228            else if ( fr.matchSequence("MOTION") )
229            {
230                ++fr;
231                buildMotion( fr, anim.get() );
232            }
233            else
234            {
235                if ( fr[0].getStr()==NULL ) continue;
236
237                OSG_WARN << "BVH Reader: Unexpected beginning " << fr[0].getStr()
238                    <<  ", neither HIERARCHY nor MOTION. Stopped." << std::endl;
239                break;
240            }
241        }
242
243#if 0
244        std::cout << "Bone number: " << _joints.size() << "\n";
245        for ( unsigned int i=0; i<_joints.size(); ++i )
246        {
247            JointNode node = _joints[i];
248            std::cout << "Bone" << i << " " << node.first->getName() << ": ";
249            std::cout << std::hex << node.second << std::dec << "; ";
250            if ( node.first->getNumParents() )
251                std::cout << "Parent: " << node.first->getParent(0)->getName();
252            std::cout << "\n";
253        }
254#endif
255
256        osg::Group* root = new osg::Group;
257        osgAnimation::BasicAnimationManager* manager = new osgAnimation::BasicAnimationManager;
258        root->addChild( skelroot.get() );
259        root->setUpdateCallback(manager);
260        manager->registerAnimation( anim.get() );
261        manager->buildTargetReference();
262        manager->playAnimation( anim.get() );
263
264        _joints.clear();
265        return root;
266    }
267
268protected:
269    void alterChannel( std::string name, int& value )
270    {
271        if      ( name=="Xposition" ) value |= 0x01;
272        else if ( name=="Yposition" ) value |= 0x02;
273        else if ( name=="Zposition" ) value |= 0x04;
274        else if ( name=="Zrotation" ) value |= 0x08;
275        else if ( name=="Xrotation" ) value |= 0x10;
276        else if ( name=="Yrotation" ) value |= 0x20;
277    }
278
279    void setKeyframe( osgDB::Input& fr, int ch, double time,
280                      osgAnimation::Vec3KeyframeContainer* posKey,
281                      osgAnimation::QuatKeyframeContainer* rotKey )
282    {
283        if ( ch&0x07 && posKey )  // Position keyframe
284        {
285            float keyValue[3] = { 0.0f };
286            if ( ch&0x01 ) fr.readSequence( keyValue[0] );
287            if ( ch&0x02 ) fr.readSequence( keyValue[1] );
288            if ( ch&0x04 ) fr.readSequence( keyValue[2] );
289
290            osg::Vec3 vec( keyValue[0], keyValue[1], keyValue[2] );
291            posKey->push_back( osgAnimation::Vec3Keyframe(time, vec) );
292        }
293
294        if ( ch&0x38 && rotKey )  // Rotation keyframe
295        {
296            float keyValue[3] = { 0.0f };
297            if ( ch&0x08 ) fr.readSequence( keyValue[2] );
298            if ( ch&0x10 ) fr.readSequence( keyValue[0] );
299            if ( ch&0x20 ) fr.readSequence( keyValue[1] );
300
301            // Note that BVH data need to concatenate the rotating matrices as Y*X*Z
302            // So we should not create Quat directly from input values, which makes a wrong X*Y*Z
303            osg::Matrix rotMat = osg::Matrix::rotate(osg::DegreesToRadians(keyValue[1]), osg::Vec3(0.0,1.0,0.0))
304                               * osg::Matrix::rotate(osg::DegreesToRadians(keyValue[0]), osg::Vec3(1.0,0.0,0.0))
305                               * osg::Matrix::rotate(osg::DegreesToRadians(keyValue[2]), osg::Vec3(0.0,0.0,1.0));
306            osg::Quat quat = rotMat.getRotate();
307            rotKey->push_back( osgAnimation::QuatKeyframe(time, quat) );
308        }
309    }
310
311    osg::ref_ptr<osg::Geode> createRefGeometry( osg::Vec3 p, double len )
312    {
313        osg::ref_ptr<osg::Geode> geode = new osg::Geode;
314
315        if ( _drawingFlag==1 )
316        {
317            osg::ref_ptr<osg::Geometry> geometry = new osg::Geometry;
318            osg::ref_ptr<osg::Vec3Array> vertices = new osg::Vec3Array;
319
320            // Joint
321            vertices->push_back( osg::Vec3(-len, 0.0, 0.0) );
322            vertices->push_back( osg::Vec3( len, 0.0, 0.0) );
323            vertices->push_back( osg::Vec3( 0.0,-len, 0.0) );
324            vertices->push_back( osg::Vec3( 0.0, len, 0.0) );
325            vertices->push_back( osg::Vec3( 0.0, 0.0,-len) );
326            vertices->push_back( osg::Vec3( 0.0, 0.0, len) );
327
328            // Bone
329            vertices->push_back( osg::Vec3( 0.0, 0.0, 0.0) );
330            vertices->push_back( p );
331
332            geometry->addPrimitiveSet( new osg::DrawArrays(osg::PrimitiveSet::LINES, 0, 8) );
333            geometry->setVertexArray( vertices.get() );
334
335            geode->addDrawable( geometry.get() );
336        }
337        else if ( _drawingFlag==2 )
338        {
339            osg::Quat quat;
340            osg::ref_ptr<osg::Box> box = new osg::Box( p*0.5, p.length(), len, len );
341            quat.makeRotate( osg::Vec3(1.0,0.0,0.0), p );
342            box->setRotation( quat );
343
344            geode->addDrawable( new osg::ShapeDrawable(box.get()) );
345        }
346
347        return geode;
348    }
349
350    int _drawingFlag;
351    JointList _joints;
352};
353
354class ReaderWriterBVH : public osgDB::ReaderWriter
355{
356public:
357    ReaderWriterBVH()
358    {
359        supportsExtension( "bvh", "Biovision motion hierarchical file" );
360
361        supportsOption( "contours","Show the skeleton with lines." );
362        supportsOption( "solids","Show the skeleton with solid boxes." );
363    }
364
365    virtual const char* className() const
366    { return "BVH Motion Reader"; }
367
368    virtual ReadResult readNode(std::istream& stream, const osgDB::ReaderWriter::Options* options) const
369    {
370        ReadResult rr = BvhMotionBuilder::instance()->buildBVH( stream, options );
371        return rr;
372    }
373
374    virtual ReadResult readNode(const std::string& file, const osgDB::ReaderWriter::Options* options) const
375    {
376        std::string ext = osgDB::getLowerCaseFileExtension( file );
377        if ( !acceptsExtension(ext) ) return ReadResult::FILE_NOT_HANDLED;
378
379        std::string fileName = osgDB::findDataFile( file, options );
380        if ( fileName.empty() ) return ReadResult::FILE_NOT_FOUND;
381
382        osgDB::ifstream stream( fileName.c_str(), std::ios::in|std::ios::binary );
383        if( !stream ) return ReadResult::ERROR_IN_READING_FILE;
384        return readNode( stream, options );
385    }
386};
387
388// Now register with Registry to instantiate the above reader/writer.
389REGISTER_OSGPLUGIN( bvh, ReaderWriterBVH )
Note: See TracBrowser for help on using the browser.