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

Revision 8868, 10.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
14#include <osgSim/Sector>
15#include <osg/Vec2>
16
17using namespace osgSim;
18
19
20//
21// Elevation Range
22//
23
24void AzimRange::setAzimuthRange(float minAzimuth,float maxAzimuth,float fadeAngle)
25{
26    // clamp the azimuth range.
27    const float twoPI = 2.0f*(float)osg::PI;
28    while(minAzimuth>maxAzimuth) minAzimuth -= twoPI;
29
30    // compute the centerline
31    float centerAzim = (minAzimuth+maxAzimuth)*0.5f;
32    _cosAzim = cos(centerAzim);
33    _sinAzim = sin(centerAzim);
34
35    // compute the half angle range of the sector.
36    float angle = (maxAzimuth-minAzimuth)*0.5f;
37    _cosAngle = cos(angle);
38
39    // clamp the fade angle to valid values.
40    fadeAngle = osg::clampAbove(fadeAngle,0.0f);
41    if (angle+fadeAngle>osg::PI) _cosFadeAngle = -1.0f;
42    else _cosFadeAngle = cos(angle+fadeAngle);
43
44}
45
46void AzimRange::getAzimuthRange(float& minAzimuth, float& maxAzimuth, float& fadeAngle) const
47{
48    float centerAzim = atan2(_sinAzim, _cosAzim);
49    float angle = acos(_cosAngle);
50    minAzimuth = centerAzim - angle;
51    maxAzimuth = centerAzim + angle;
52    if (_cosFadeAngle == -1.0f) {
53        fadeAngle = 2.0f * osg::PI;
54    } else {
55        fadeAngle = acos(_cosFadeAngle) - angle;
56    }
57}
58
59
60//
61// Elevation Range
62//
63void ElevationRange::setElevationRange(float minElevation,float maxElevation,float fadeAngle)
64{
65    if (minElevation>maxElevation)
66    {
67        // need to swap angle pair.
68        float tmp = minElevation;
69        minElevation = maxElevation;
70        maxElevation = tmp;
71    }
72
73    minElevation = osg::clampTo(minElevation,(float)-osg::PI_2,(float)osg::PI_2);
74    maxElevation = osg::clampTo(maxElevation,(float)-osg::PI_2,(float)osg::PI_2);
75    fadeAngle = osg::clampTo(fadeAngle,0.0f,(float)osg::PI_2);
76
77    _cosMinElevation = cos(osg::PI_2-minElevation);
78    _cosMaxElevation = cos(osg::PI_2-maxElevation);
79
80    float minFadeAngle = osg::PI_2-minElevation+fadeAngle;
81    if (minFadeAngle>=osg::PI) _cosMinFadeElevation = -1.0f;
82    else _cosMinFadeElevation = cos(minFadeAngle);
83
84    float maxFadeAngle = osg::PI_2-maxElevation-fadeAngle;
85    if (maxFadeAngle<=0.0f) _cosMaxFadeElevation = 1.0f;
86    else _cosMaxFadeElevation = cos(maxFadeAngle);
87
88}
89
90float ElevationRange::getMinElevation() const
91{
92    return osg::PI_2-acos(_cosMinElevation);
93}
94
95float ElevationRange::getMaxElevation() const
96{
97    return osg::PI_2-acos(_cosMaxElevation);
98}
99
100float ElevationRange::getFadeAngle() const
101{
102    float fadeAngle = 0.0;
103
104    // Take the appropriate (unclipped) elevation angle to calculate the fade angle
105    if (_cosMinFadeElevation != -1.0f) {
106        float minFadeAngle = acos(_cosMinFadeElevation);
107        float minElevation = osg::PI_2 - acos(_cosMinElevation);
108        fadeAngle = minFadeAngle + minElevation - osg::PI_2;
109
110    } else if (_cosMaxFadeElevation != 1.0f) {
111        float maxFadeAngle = acos(_cosMaxFadeElevation);
112        float maxElevation = osg::PI_2 - acos(_cosMaxElevation);
113        fadeAngle = osg::PI_2 - maxFadeAngle - maxElevation;
114    }
115
116    return fadeAngle;
117}
118
119//
120// ElevationSector
121//
122AzimSector::AzimSector(float minAzimuth,float maxAzimuth,float fadeAngle):
123    Sector(),
124    AzimRange()
125{
126    setAzimuthRange(minAzimuth,maxAzimuth,fadeAngle);
127}
128
129float AzimSector::operator() (const osg::Vec3& eyeLocal) const
130{
131    return azimSector(eyeLocal);
132}
133
134//
135// ElevationSector
136//
137ElevationSector::ElevationSector(float minElevation,float maxElevation,float fadeAngle):
138    Sector(),
139    ElevationRange()
140{
141    setElevationRange(minElevation,maxElevation,fadeAngle);
142}
143
144float ElevationSector::operator() (const osg::Vec3& eyeLocal) const
145{
146    return elevationSector(eyeLocal);
147}
148
149//
150// AzimElevationSector
151//
152AzimElevationSector::AzimElevationSector(float minAzimuth,float maxAzimuth,float minElevation,float maxElevation,float fadeAngle):
153    Sector(),
154    AzimRange(),
155    ElevationRange()
156{
157    setAzimuthRange(minAzimuth,maxAzimuth,fadeAngle);
158    setElevationRange(minElevation,maxElevation,fadeAngle);
159}
160
161
162float AzimElevationSector::operator() (const osg::Vec3& eyeLocal) const
163{
164    float azimIntensity = azimSector(eyeLocal);
165    if (azimIntensity==0.0) return 0.0; // out of sector.
166    float elevIntensity = elevationSector(eyeLocal);
167    if (elevIntensity==0.0) return 0.0; // out of sector.
168    if (azimIntensity<=elevIntensity) return azimIntensity;
169    return elevIntensity;
170}
171
172//
173// ConeSector
174//
175ConeSector::ConeSector(const osg::Vec3& axis,float angle,float fadeangle):
176            Sector()
177{
178    setAxis(axis);
179    setAngle(angle,fadeangle);
180}
181
182void ConeSector::setAxis(const osg::Vec3& axis)
183{
184    _axis = axis;
185    _axis.normalize();
186}
187
188const osg::Vec3& ConeSector::getAxis() const
189{
190    return _axis;
191}
192
193void ConeSector::setAngle(float angle,float fadeangle)
194{
195    _cosAngle = cos(angle);
196    _cosAngleFade = cos(angle+fadeangle);
197}
198
199float ConeSector::getAngle() const
200{
201    return acos(_cosAngle);
202}
203
204float ConeSector::getFadeAngle() const
205{
206    return acos(_cosAngleFade)-acos(_cosAngle);
207}
208
209float ConeSector::operator() (const osg::Vec3& eyeLocal) const
210{
211    float dotproduct = eyeLocal*_axis;
212    float length = eyeLocal.length();
213    if (dotproduct>_cosAngle*length) return 1.0f; // fully in sector
214    if (dotproduct<_cosAngleFade*length) return 0.0f; // out of sector
215    return (dotproduct-_cosAngleFade*length)/((_cosAngle-_cosAngleFade)*length);
216}
217
218//
219// DirectionalSector
220//
221DirectionalSector::DirectionalSector(const osg::Vec3& direction,float horizLobeAngle, float vertLobeAngle, float lobeRollAngle, float fadeAngle):
222            Sector()
223{
224    setDirection(direction);
225    setHorizLobeAngle(horizLobeAngle);
226    setVertLobeAngle(vertLobeAngle);
227    setLobeRollAngle(lobeRollAngle);
228    setFadeAngle(fadeAngle);
229}
230
231void DirectionalSector::computeMatrix()
232{
233  double heading = atan2(_direction[0], _direction[1]);
234  double pitch   = atan2(_direction[2], sqrt(_direction[0]*_direction[0] + _direction[1]*_direction[1]));
235  double roll    = _rollAngle;
236
237  _local_to_LP.setRotate(osg::Quat(heading, 0.0, 0.0, -1.0));
238  _local_to_LP.preMultRotate(osg::Quat(pitch, 1.0, 0.0, 0.0));
239  _local_to_LP.preMultRotate(osg::Quat(roll, 0.0, 1.0, 0.0));
240}
241
242void DirectionalSector::setDirection(const osg::Vec3& direction)
243{
244   _direction = direction ;
245   computeMatrix() ;
246}
247
248const osg::Vec3& DirectionalSector::getDirection() const
249{
250    return _direction;
251}
252
253void DirectionalSector::setHorizLobeAngle(float angle)
254{
255    _cosHorizAngle = cos(angle*0.5);
256}
257
258float DirectionalSector::getHorizLobeAngle() const
259{
260    return acos(_cosHorizAngle)*2.0;
261}
262
263void DirectionalSector::setVertLobeAngle(float angle)
264{
265    _cosVertAngle = cos(angle*0.5);
266}
267
268float DirectionalSector::getVertLobeAngle() const
269{
270    return acos(_cosVertAngle)*2.0;
271}
272
273void DirectionalSector::setLobeRollAngle(float angle)
274{
275    _rollAngle = angle ;
276    computeMatrix() ;
277}
278
279float DirectionalSector::getLobeRollAngle() const
280{
281    return _rollAngle ;
282}
283
284void DirectionalSector::setFadeAngle(float angle)
285{
286    float ang = acos(_cosHorizAngle)+angle ;
287    if ( ang > osg::PI ) _cosHorizFadeAngle = -1.0 ;
288    else _cosHorizFadeAngle = cos(ang);
289   
290    ang = acos(_cosVertAngle)+angle ;
291    if ( ang > osg::PI ) _cosVertFadeAngle = -1.0 ;
292    else _cosVertFadeAngle = cos(ang);
293}
294
295float DirectionalSector::getFadeAngle() const
296{
297    return acos(_cosHorizFadeAngle)-acos(_cosHorizAngle);
298}
299
300float DirectionalSector::operator() (const osg::Vec3& eyeLocal) const
301{     
302   float elev_intensity, azim_intensity ;
303   
304   // Tranform eyeLocal into the LightPoint frame
305   osg::Vec3 EPlp = _local_to_LP * eyeLocal ;
306   
307   /*fprintf(stderr, "    eyeLocal = %f, %f, %f\n", eyeLocal[0], eyeLocal[1], eyeLocal[2]) ;
308   fprintf(stderr, "    EPlp     = %f, %f, %f\n", EPlp[0], EPlp[1], EPlp[2]) ;*/
309   
310   // Elevation check
311     // Project EPlp into LP YZ plane and dot with LPy
312   osg::Vec2 EPyz(EPlp[1], EPlp[2]) ;
313   EPyz.normalize() ;
314   /*fprintf(stderr, "    EPyz.normalize() = %f, %f\n", EPyz[0], EPyz[1]) ;
315   fprintf(stderr, "        _cosVertFadeAngle = %f\n", _cosVertFadeAngle) ;
316   fprintf(stderr, "        _cosVertAngle     = %f\n", _cosVertAngle) ;*/
317      // cosElev = EPyz* LPy = EPyz[0]
318   if ( EPyz[0] < _cosVertFadeAngle ) {
319      // Completely outside elevation range
320      //fprintf(stderr, "   >> outside el range\n") ;
321      return(0.0f) ;
322   }
323   if ( EPyz[0] < _cosVertAngle ) {
324      // In the fade range
325      //fprintf(stderr, "   >> inside el fade range\n") ;
326      elev_intensity = (_cosVertAngle-EPyz[0])/(_cosVertAngle-_cosVertFadeAngle) ;
327   } else {
328      // Fully in elevation range
329      elev_intensity = 1.0 ;
330      //fprintf(stderr, "   >> fully inside el range\n") ;
331   }
332   // Elevation check passed
333   
334   // Azimuth check
335     // Project EPlp into LP XY plane and dot with LPy
336   osg::Vec2 EPxy(EPlp[0], EPlp[1]) ;
337   EPxy.normalize() ;
338   /*fprintf(stderr, "    EPxy.normalize() = %f, %f\n", EPxy[0], EPxy[1]) ;
339   fprintf(stderr, "        _cosHorizFadeAngle = %f\n", _cosHorizFadeAngle) ;
340   fprintf(stderr, "        _cosHorizAngle     = %f\n", _cosHorizAngle) ;*/
341      // cosAzim = EPxy * LPy = EPxy[1]
342      // if cosElev < 0.0, then need to negate EP for azimuth check
343   if ( EPyz[0] < 0.0 ) EPxy.set(-EPxy[0], -EPxy[1]) ;
344   if ( EPxy[1] < _cosHorizFadeAngle ) {
345      // Completely outside azimuth range
346      //fprintf(stderr, "   >> outside az range\n") ;
347      return(0.0f) ;
348   }
349   if ( EPxy[1] < _cosHorizAngle ) {
350      // In fade range
351      //fprintf(stderr, "   >> inside az fade range\n") ;
352      azim_intensity = (_cosHorizAngle-EPxy[1])/(_cosHorizAngle-_cosHorizFadeAngle) ;
353   } else {
354      // Fully in azimuth range
355      //fprintf(stderr, "   >> fully inside az range\n") ;
356      azim_intensity = 1.0 ;
357   }
358   // Azimuth check passed
359   
360   // We're good! Return full intensity
361   //fprintf(stderr, "   %%%% Returing intensity = %f\n", elev_intensity * azim_intensity) ;
362   return elev_intensity * azim_intensity ;
363}
Note: See TracBrowser for help on using the browser.