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

Revision 8370, 10.4 kB (checked in by robert, 7 years ago)

From Lars Nilson, bug fix to Sector::computeMatrix()

  • 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 = osg::Matrixd::identity();
238  _local_to_LP.preMult(osg::Matrix::rotate(heading, 0.0, 0.0, -1.0));
239  _local_to_LP.preMult(osg::Matrix::rotate(pitch, 1.0, 0.0, 0.0));
240  _local_to_LP.preMult(osg::Matrix::rotate(roll, 0.0, 1.0, 0.0));
241}
242
243void DirectionalSector::setDirection(const osg::Vec3& direction)
244{
245   _direction = direction ;
246   computeMatrix() ;
247}
248
249const osg::Vec3& DirectionalSector::getDirection() const
250{
251    return _direction;
252}
253
254void DirectionalSector::setHorizLobeAngle(float angle)
255{
256    _cosHorizAngle = cos(angle*0.5);
257}
258
259float DirectionalSector::getHorizLobeAngle() const
260{
261    return acos(_cosHorizAngle)*2.0;
262}
263
264void DirectionalSector::setVertLobeAngle(float angle)
265{
266    _cosVertAngle = cos(angle*0.5);
267}
268
269float DirectionalSector::getVertLobeAngle() const
270{
271    return acos(_cosVertAngle)*2.0;
272}
273
274void DirectionalSector::setLobeRollAngle(float angle)
275{
276    _rollAngle = angle ;
277    computeMatrix() ;
278}
279
280float DirectionalSector::getLobeRollAngle() const
281{
282    return _rollAngle ;
283}
284
285void DirectionalSector::setFadeAngle(float angle)
286{
287    float ang = acos(_cosHorizAngle)+angle ;
288    if ( ang > osg::PI ) _cosHorizFadeAngle = -1.0 ;
289    else _cosHorizFadeAngle = cos(ang);
290   
291    ang = acos(_cosVertAngle)+angle ;
292    if ( ang > osg::PI ) _cosVertFadeAngle = -1.0 ;
293    else _cosVertFadeAngle = cos(ang);
294}
295
296float DirectionalSector::getFadeAngle() const
297{
298    return acos(_cosHorizFadeAngle)-acos(_cosHorizAngle);
299}
300
301float DirectionalSector::operator() (const osg::Vec3& eyeLocal) const
302{     
303   float elev_intensity, azim_intensity ;
304   
305   // Tranform eyeLocal into the LightPoint frame
306   osg::Vec3 EPlp = _local_to_LP * eyeLocal ;
307   
308   /*fprintf(stderr, "    eyeLocal = %f, %f, %f\n", eyeLocal[0], eyeLocal[1], eyeLocal[2]) ;
309   fprintf(stderr, "    EPlp     = %f, %f, %f\n", EPlp[0], EPlp[1], EPlp[2]) ;*/
310   
311   // Elevation check
312     // Project EPlp into LP YZ plane and dot with LPy
313   osg::Vec2 EPyz(EPlp[1], EPlp[2]) ;
314   EPyz.normalize() ;
315   /*fprintf(stderr, "    EPyz.normalize() = %f, %f\n", EPyz[0], EPyz[1]) ;
316   fprintf(stderr, "        _cosVertFadeAngle = %f\n", _cosVertFadeAngle) ;
317   fprintf(stderr, "        _cosVertAngle     = %f\n", _cosVertAngle) ;*/
318      // cosElev = EPyz* LPy = EPyz[0]
319   if ( EPyz[0] < _cosVertFadeAngle ) {
320      // Completely outside elevation range
321      //fprintf(stderr, "   >> outside el range\n") ;
322      return(0.0f) ;
323   }
324   if ( EPyz[0] < _cosVertAngle ) {
325      // In the fade range
326      //fprintf(stderr, "   >> inside el fade range\n") ;
327      elev_intensity = (_cosVertAngle-EPyz[0])/(_cosVertAngle-_cosVertFadeAngle) ;
328   } else {
329      // Fully in elevation range
330      elev_intensity = 1.0 ;
331      //fprintf(stderr, "   >> fully inside el range\n") ;
332   }
333   // Elevation check passed
334   
335   // Azimuth check
336     // Project EPlp into LP XY plane and dot with LPy
337   osg::Vec2 EPxy(EPlp[0], EPlp[1]) ;
338   EPxy.normalize() ;
339   /*fprintf(stderr, "    EPxy.normalize() = %f, %f\n", EPxy[0], EPxy[1]) ;
340   fprintf(stderr, "        _cosHorizFadeAngle = %f\n", _cosHorizFadeAngle) ;
341   fprintf(stderr, "        _cosHorizAngle     = %f\n", _cosHorizAngle) ;*/
342      // cosAzim = EPxy * LPy = EPxy[1]
343      // if cosElev < 0.0, then need to negate EP for azimuth check
344   if ( EPyz[0] < 0.0 ) EPxy.set(-EPxy[0], -EPxy[1]) ;
345   if ( EPxy[1] < _cosHorizFadeAngle ) {
346      // Completely outside azimuth range
347      //fprintf(stderr, "   >> outside az range\n") ;
348      return(0.0f) ;
349   }
350   if ( EPxy[1] < _cosHorizAngle ) {
351      // In fade range
352      //fprintf(stderr, "   >> inside az fade range\n") ;
353      azim_intensity = (_cosHorizAngle-EPxy[1])/(_cosHorizAngle-_cosHorizFadeAngle) ;
354   } else {
355      // Fully in azimuth range
356      //fprintf(stderr, "   >> fully inside az range\n") ;
357      azim_intensity = 1.0 ;
358   }
359   // Azimuth check passed
360   
361   // We're good! Return full intensity
362   //fprintf(stderr, "   %%%% Returing intensity = %f\n", elev_intensity * azim_intensity) ;
363   return elev_intensity * azim_intensity ;
364}
Note: See TracBrowser for help on using the browser.