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

Revision 8868, 16.3 kB (checked in by robert, 6 years ago)

From Mathias Froehlich, "This is a generic optimization that does not depend on any cpu or instruction
set.

The optimization is based on the observation that matrix matrix multiplication
with a dense matrix 4x4 is 43 Operations whereas multiplication with a
transform, or scale matrix is only 4
2 operations. Which is a gain of a
*FACTOR*4* for these special cases.
The change implements these special cases, provides a unit test for these
implementation and converts uses of the expensiver dense matrix matrix
routine with the specialized versions.

Depending on the transform nodes in the scenegraph this change gives a
noticable improovement.
For example the osgforest code using the MatrixTransform? is about 20% slower
than the same codepath using the PositionAttitudeTransform? instead of the
MatrixTransform? with this patch applied.

If I remember right, the sse type optimizations did *not* provide a factor 4
improovement. Also these changes are totally independent of any cpu or
instruction set architecture. So I would prefer to have this current kind of
change instead of some hand coded and cpu dependent assembly stuff. If we
need that hand tuned stuff, these can go on top of this changes which must
provide than hand optimized additional variants for the specialized versions
to give a even better result in the end.

An other change included here is a change to rotation matrix from quaterion
code. There is a sqrt call which couold be optimized away. Since we divide in
effect by sqrt(length)*sqrt(length) which is just length ...
"

  • 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(-1),
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.