root/OpenSceneGraph/trunk/src/osgSim/DOFTransform.cpp @ 13041

Revision 13041, 16.3 kB (checked in by robert, 14 months ago)

Ran script to remove trailing spaces and tabs

  • Property svn:eol-style set to native
  • Property svn:keywords set to Author Date Id Revision
Line 
1/* -*-c++-*- OpenSceneGraph - Copyright (C) 1998-2006 Robert Osfield
2 *
3 * This library is open source and may be redistributed and/or modified under
4 * the terms of the OpenSceneGraph Public License (OSGPL) version 0.0 or
5 * (at your option) any later version.  The full license is in LICENSE file
6 * included with this distribution, and on the openscenegraph.org website.
7 *
8 * This library is distributed in the hope that it will be useful,
9 * but WITHOUT ANY WARRANTY; without even the implied warranty of
10 * MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
11 * OpenSceneGraph Public License for more details.
12*/
13#include <osgSim/DOFTransform>
14
15using namespace osgSim;
16using namespace osg;
17
18static const unsigned int TRANSLATION_X_LIMIT_BIT  = 0x80000000u >> 0;
19static const unsigned int TRANSLATION_Y_LIMIT_BIT  = 0x80000000u >> 1;
20static const unsigned int TRANSLATION_Z_LIMIT_BIT  = 0x80000000u >> 2;
21static const unsigned int ROTATION_PITCH_LIMIT_BIT = 0x80000000u >> 3;
22static const unsigned int ROTATION_ROLL_LIMIT_BIT  = 0x80000000u >> 4;
23static const unsigned int ROTATION_YAW_LIMIT_BIT   = 0x80000000u >> 5;
24static const unsigned int SCALE_X_LIMIT_BIT        = 0x80000000u >> 6;
25static const unsigned int SCALE_Y_LIMIT_BIT        = 0x80000000u >> 7;
26static const unsigned int SCALE_Z_LIMIT_BIT        = 0x80000000u >> 8;
27
28
29DOFTransform::DOFTransform():
30    _previousTraversalNumber(osg::UNINITIALIZED_FRAME_NUMBER),
31    _previousTime(0.0),
32    _limitationFlags(0),
33    _animationOn(false),
34    _increasingFlags(0xffff),
35    _multOrder(PRH)
36{
37}
38
39DOFTransform::DOFTransform(const DOFTransform& dof, const osg::CopyOp& copyop):
40    osg::Transform(dof, copyop),
41    _previousTraversalNumber(dof._previousTraversalNumber),
42    _previousTime(dof._previousTime),
43    _minHPR(dof._minHPR),
44    _maxHPR(dof._maxHPR),
45    _currentHPR(dof._currentHPR),
46    _incrementHPR(dof._incrementHPR),
47    _minTranslate(dof._minTranslate),
48    _maxTranslate(dof._maxTranslate),
49    _currentTranslate(dof._currentTranslate),
50    _incrementTranslate(dof._incrementTranslate),
51    _minScale(dof._minScale),
52    _maxScale(dof._maxScale),
53    _currentScale(dof._currentScale),
54    _incrementScale(dof._incrementScale),
55    _Put(dof._Put),
56    _inversePut(dof._inversePut),
57    _limitationFlags(dof._limitationFlags),
58    _animationOn(dof._animationOn),
59    _increasingFlags(dof._increasingFlags),
60    _multOrder(dof._multOrder)
61{
62    if (_animationOn) setNumChildrenRequiringUpdateTraversal(getNumChildrenRequiringUpdateTraversal()+1);
63}
64
65void DOFTransform::traverse(osg::NodeVisitor& nv)
66{
67    if (nv.getVisitorType()==osg::NodeVisitor::UPDATE_VISITOR)
68    {
69        // ensure that we do not operate on this node more than
70        // once during this traversal.  This is an issue since node
71        // can be shared between multiple parents.
72        if ((nv.getTraversalNumber()!=_previousTraversalNumber) && nv.getFrameStamp())
73        {
74            double newTime = nv.getFrameStamp()->getSimulationTime();
75
76            animate((float)(newTime-_previousTime));
77
78            _previousTraversalNumber = nv.getTraversalNumber();
79            _previousTime = newTime;
80        }
81    }
82
83    Transform::traverse(nv);
84}
85
86bool DOFTransform::computeLocalToWorldMatrix(osg::Matrix& matrix,osg::NodeVisitor*) const
87{
88    //put the PUT matrix first:
89    osg::Matrix l2w(getPutMatrix());
90
91    //now the current matrix:
92    osg::Matrix current;
93    current.makeTranslate(getCurrentTranslate());
94
95    //now create the local rotation:
96    if(_multOrder == PRH)
97    {
98        current.preMult(osg::Matrix::rotate(getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
99        current.preMult(osg::Matrix::rotate(getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
100        current.preMult(osg::Matrix::rotate(getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
101    }
102    else if(_multOrder == PHR)
103    {
104        current.preMult(osg::Matrix::rotate(getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
105        current.preMult(osg::Matrix::rotate(getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
106        current.preMult(osg::Matrix::rotate(getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
107    }
108    else if(_multOrder == HPR)
109    {
110        current.preMult(osg::Matrix::rotate(getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
111        current.preMult(osg::Matrix::rotate(getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
112        current.preMult(osg::Matrix::rotate(getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
113    }
114    else if(_multOrder == HRP)
115    {
116        current.preMult(osg::Matrix::rotate(getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
117        current.preMult(osg::Matrix::rotate(getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
118        current.preMult(osg::Matrix::rotate(getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
119    }
120    else if(_multOrder == RHP)
121    {
122        current.preMult(osg::Matrix::rotate(getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
123        current.preMult(osg::Matrix::rotate(getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
124        current.preMult(osg::Matrix::rotate(getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
125    }
126    else // _multOrder == RPH
127    {
128        current.preMult(osg::Matrix::rotate(getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
129        current.preMult(osg::Matrix::rotate(getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
130        current.preMult(osg::Matrix::rotate(getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
131    }
132
133
134    //and scale:
135    current.preMultScale(getCurrentScale());
136
137    l2w.postMult(current);
138
139    //and impose inverse put:
140    l2w.postMult(getInversePutMatrix());
141
142    // finally.
143    if (_referenceFrame==RELATIVE_RF)
144    {
145        matrix.preMult(l2w);
146    }
147    else
148    {
149        matrix = l2w;
150    }
151
152    return true;
153}
154
155
156bool DOFTransform::computeWorldToLocalMatrix(osg::Matrix& matrix,osg::NodeVisitor*) const
157{
158    //put the PUT matrix first:
159    osg::Matrix w2l(getInversePutMatrix());
160
161    //now the current matrix:
162    osg::Matrix current;
163    current.makeTranslate(-getCurrentTranslate());
164
165    //now create the local rotation:
166    if(_multOrder == PRH)
167    {
168        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
169        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
170        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
171    }
172    else if(_multOrder == PHR)
173    {
174        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
175        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
176        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
177    }
178    else if(_multOrder == HPR)
179    {
180        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
181        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
182        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
183    }
184    else if(_multOrder == HRP)
185    {
186        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
187        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
188        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
189    }
190    else if(_multOrder == RHP)
191    {
192        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
193        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
194        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
195    }
196    else // _multOrder == MultOrder::RPH
197    {
198        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[0], 0.0, 0.0, 1.0));//heading
199        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[1], 1.0, 0.0, 0.0));//pitch
200        current.postMult(osg::Matrix::rotate(-getCurrentHPR()[2], 0.0, 1.0, 0.0));//roll
201    }
202
203    //and scale:
204    current.postMultScale(osg::Vec3d(1./getCurrentScale()[0], 1./getCurrentScale()[1], 1./getCurrentScale()[2]));
205
206    w2l.postMult(current);
207
208    //and impose inverse put:
209    w2l.postMult(getPutMatrix());
210
211    if (_referenceFrame==RELATIVE_RF)
212    {
213        //finally:
214        matrix.postMult(w2l);
215    }
216    else // absolute
217    {
218        matrix = w2l;
219    }
220    return true;
221}
222
223
224void DOFTransform::updateCurrentHPR(const osg::Vec3& hpr)
225{
226    //if there is constrain on animation
227    if (_limitationFlags & ROTATION_ROLL_LIMIT_BIT)
228    {
229        //if we have min == max, it is efective constrain, so don't change
230        if(_minHPR[2] != _maxHPR[2])
231        {
232            _currentHPR[2] = hpr[2];
233            unsigned short this_flag = (unsigned short)1<<4;//roll
234
235            if(_currentHPR[2] < _minHPR[2])
236            {
237                _currentHPR[2] = _minHPR[2];
238                //force increasing flag to 1
239                _increasingFlags |= this_flag;
240            }
241            else if(_currentHPR[2] > _maxHPR[2])
242            {
243                _currentHPR[2] = _maxHPR[2];
244                //force increasing flag to 0
245                _increasingFlags &= ~this_flag;
246            }
247        }
248    }
249    else
250    {
251        _currentHPR[2] = hpr[2];
252    }
253
254    if (_limitationFlags & ROTATION_PITCH_LIMIT_BIT)
255    {
256        if(_minHPR[1] != _maxHPR[1])
257        {
258            _currentHPR[1] = hpr[1];
259            unsigned short this_flag = (unsigned short)1<<3;//pitch
260
261            if(_currentHPR[1] < _minHPR[1])
262            {
263                _currentHPR[1] = _minHPR[1];
264                _increasingFlags |= this_flag;
265            }
266            else if(_currentHPR[1] > _maxHPR[1])
267            {
268                _currentHPR[1] = _maxHPR[1];
269                _increasingFlags &= ~this_flag;
270            }
271        }
272    }
273    else
274    {
275        _currentHPR[1] = hpr[1];
276    }
277
278    if (_limitationFlags & ROTATION_YAW_LIMIT_BIT)
279    {
280        if(_minHPR[0] != _maxHPR[0])
281        {
282            _currentHPR[0] = hpr[0];
283
284            unsigned short this_flag = (unsigned short)1<<5;//heading
285
286            if(_currentHPR[0] < _minHPR[0])
287            {
288                _currentHPR[0] = _minHPR[0];
289                _increasingFlags |= this_flag;
290            }
291            else if(_currentHPR[0] > _maxHPR[0])
292            {
293                _currentHPR[0] = _maxHPR[0];
294                _increasingFlags &= ~this_flag;
295            }
296        }
297    }
298    else
299    {
300        _currentHPR[0] = hpr[0];
301    }
302
303    dirtyBound();
304}
305
306
307void DOFTransform::updateCurrentTranslate(const osg::Vec3& translate)
308{
309    if (_limitationFlags & TRANSLATION_Z_LIMIT_BIT)
310    {
311        if(_minTranslate[2] != _maxTranslate[2])
312        {
313            _currentTranslate[2] = translate[2];
314            unsigned short this_flag = (unsigned short)1<<2;
315
316            if(_currentTranslate[2] < _minTranslate[2])
317            {
318                _currentTranslate[2] = _minTranslate[2];
319                _increasingFlags |= this_flag;
320            }
321            else if(_currentTranslate[2] > _maxTranslate[2])
322            {
323                _currentTranslate[2] = _maxTranslate[2];
324                _increasingFlags &= ~this_flag;
325            }
326        }
327    }
328    else
329    {
330        _currentTranslate[2] = translate[2];
331    }
332
333    if (_limitationFlags & TRANSLATION_Y_LIMIT_BIT)
334    {
335        if(_minTranslate[1] != _maxTranslate[1])
336        {
337            _currentTranslate[1] = translate[1];
338            unsigned short this_flag = (unsigned short)1<<1;
339
340            if(_currentTranslate[1] < _minTranslate[1])
341            {
342                _currentTranslate[1] = _minTranslate[1];
343                _increasingFlags |= this_flag;
344            }
345            else if(_currentTranslate[1] > _maxTranslate[1])
346            {
347                _currentTranslate[1] = _maxTranslate[1];
348                _increasingFlags &= ~this_flag;
349            }
350        }
351    }
352    else
353    {
354        _currentTranslate[1] = translate[1];
355    }
356
357    if (_limitationFlags & TRANSLATION_X_LIMIT_BIT)
358    {
359        if(_minTranslate[0] != _maxTranslate[0])
360        {
361            _currentTranslate[0] = translate[0];
362            unsigned short this_flag = (unsigned short)1;
363
364            if(_currentTranslate[0] < _minTranslate[0])
365            {
366                _currentTranslate[0] = _minTranslate[0];
367                _increasingFlags |= this_flag;
368            }
369            else if(_currentTranslate[0] > _maxTranslate[0])
370            {
371                _currentTranslate[0] = _maxTranslate[0];
372                _increasingFlags &= ~this_flag;
373            }
374        }
375    }
376    else
377    {
378        _currentTranslate[0] = translate[0];
379    }
380
381    dirtyBound();
382}
383
384
385void DOFTransform::updateCurrentScale(const osg::Vec3& scale)
386{
387    if (_limitationFlags & SCALE_Z_LIMIT_BIT)
388    {
389        if(_minScale[2] != _maxScale[2])
390        {
391            _currentScale[2] = scale[2];
392            unsigned short this_flag = (unsigned short)1<<8;
393
394            if(_currentScale[2] < _minScale[2])
395            {
396                _currentScale[2] = _minScale[2];
397                _increasingFlags |= this_flag;
398            }
399            else if(_currentScale[2] > _maxScale[2])
400            {
401                _currentScale[2] = _maxScale[2];
402                _increasingFlags &= ~this_flag;
403            }
404        }
405    }
406    else
407    {
408        _currentScale[2] = scale[2];
409    }
410
411    if (_limitationFlags & SCALE_Y_LIMIT_BIT)
412    {
413        if(_minScale[1] != _maxScale[1])
414        {
415            _currentScale[1] = scale[1];
416            unsigned short this_flag = (unsigned short)1<<7;
417
418            if(_currentScale[1] < _minScale[1])
419            {
420                _currentScale[1] = _minScale[1];
421                _increasingFlags |= this_flag;
422            }
423            else if(_currentScale[1] > _maxScale[1])
424            {
425                _currentScale[1] = _maxScale[1];
426                _increasingFlags &= ~this_flag;
427            }
428        }
429    }
430    else
431    {
432        _currentScale[1] = scale[1];
433    }
434
435    if (_limitationFlags & SCALE_X_LIMIT_BIT)
436    {
437        if(_minScale[0] != _maxScale[0])
438        {
439            _currentScale[0] = scale[0];
440            unsigned short this_flag = (unsigned short)1<<6;
441
442            if(_currentScale[0] < _minScale[0])
443            {
444                _currentScale[0] = _minScale[0];
445                _increasingFlags |= this_flag;
446            }
447            else if(_currentScale[0] > _maxScale[0])
448            {
449                _currentScale[0] = _maxScale[0];
450                _increasingFlags &= ~this_flag;
451            }
452        }
453    }
454    else
455    {
456        _currentScale[0] = scale[0];
457    }
458
459    dirtyBound();
460}
461
462void DOFTransform::setAnimationOn(bool do_animate)
463{
464    if (_animationOn == do_animate) return;
465
466    int delta = 0;
467
468    if (_animationOn) --delta;
469    if (do_animate) ++delta;
470
471    _animationOn = do_animate;
472
473    if (_animationOn) setNumChildrenRequiringUpdateTraversal(getNumChildrenRequiringUpdateTraversal()+delta);
474}
475
476void DOFTransform::animate(float deltaTime)
477{
478    if(!_animationOn) return;
479    //this will increment or decrement all allowed values
480
481    osg::Vec3 new_value = _currentTranslate;
482
483    if(_increasingFlags & 1)
484        new_value[0] += _incrementTranslate[0]*deltaTime;
485    else
486        new_value[0] -= _incrementTranslate[0]*deltaTime;
487
488    if(_increasingFlags & 1<<1)
489        new_value[1] += _incrementTranslate[1]*deltaTime;
490    else
491        new_value[1] -= _incrementTranslate[1]*deltaTime;
492
493    if(_increasingFlags & 1<<2)
494        new_value[2] += _incrementTranslate[2]*deltaTime;
495    else
496        new_value[2] -= _incrementTranslate[2]*deltaTime;
497
498    updateCurrentTranslate(new_value);
499
500    new_value = _currentHPR;
501
502    if(_increasingFlags & ((unsigned short)1<<3))
503        new_value[1] += _incrementHPR[1]*deltaTime;
504    else
505        new_value[1] -= _incrementHPR[1]*deltaTime;
506
507    if(_increasingFlags & ((unsigned short)1<<4))
508        new_value[2] += _incrementHPR[2]*deltaTime;
509    else
510        new_value[2] -= _incrementHPR[2]*deltaTime;
511
512    if(_increasingFlags & ((unsigned short)1<<5))
513        new_value[0] += _incrementHPR[0]*deltaTime;
514    else
515        new_value[0] -= _incrementHPR[0]*deltaTime;
516
517    updateCurrentHPR(new_value);
518
519    new_value = _currentScale;
520
521    if(_increasingFlags & 1<<6)
522        new_value[0] += _incrementScale[0]*deltaTime;
523    else
524        new_value[0] -= _incrementScale[0]*deltaTime;
525
526    if(_increasingFlags & 1<<7)
527        new_value[1] += _incrementScale[1]*deltaTime;
528    else
529        new_value[1] -= _incrementScale[1]*deltaTime;
530
531    if(_increasingFlags & 1<<8)
532        new_value[2] += _incrementScale[2]*deltaTime;
533    else
534        new_value[2] -= _incrementScale[2]*deltaTime;
535
536    updateCurrentScale(new_value);
537
538}
Note: See TracBrowser for help on using the browser.