EulerAngles
RotationMatrix3<T>::GetEulerAngles( EulerAngles::EOrder order )
{
Assert( order <= EulerAngles::YXZ );
static const int indices[6][3]
= { { 0, 1, 2 }, { 1, 2, 0 }, { 2, 0, 1 },
{ 0, 2, 1 }, { 2, 1, 0 }, { 1, 0, 2 } };
const int i0 = indices[ order ][ 0 ];
const int i1 = indices[ order ][ 1 ];
const int i2 = indices[ order ][ 2 ];
T c1 = std::sqrt( m_matrix( i0, i0 ) * m_matrix( i0, i0 )
+ m_matrix( i0, i1 ) * m_matrix( i0, i1 ) );
Angle angles[3];
if ( order <= EulerAngles::ZXY ) //even permutation
{
angles[1] = ArcTan( m_matrix( i0, i2 ), c1 );
if ( c1 > 16. * std::numeric_limits<T>::epsilon() )
{
angles[0] = ArcTan( - m_matrix( i1, i2 ), m_matrix( i2, i2 ) );
angles[2] = ArcTan( - m_matrix( i0, i1 ), m_matrix( i0, i0 ) );
}
else
{
if ( angles[1].Radians() > 0. )
//"gimble lock", only (angles[0] + angles[2]) determined
angles[0] = ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
else
//"gimble lock", only (angles[0] - angles[2]) determined
angles[0] = - ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
angles[2] = 0.;
}
}
else //odd permutation
{
angles[1] = ArcTan( - m_matrix( i0, i2 ), c1 );
if ( c1 > 16. * std::numeric_limits<T>::epsilon() )
{
angles[0] = ArcTan( m_matrix( i1, i2 ), m_matrix( i2, i2 ) );
angles[2] = ArcTan( m_matrix( i0, i1 ), m_matrix( i0, i0 ) );
}
else
{
if ( angles[1].Radians() > 0. )
//"gimble lock", only (angles[0] - angles[2]) determined
angles[0] = ArcTan( m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
else
//"gimble lock", only (angles[0] + angles[2]) determined
angles[0] = ArcTan( - m_matrix( i1, i0 ), m_matrix( i1, i1 ) );
angles[2] = 0.;
}
}
return EulerAngles( angles );
}
void
Geodetic::Set( const Point3D & rectangular )
{
//Explanatory Supplement (4.22-11 to 4.22-24)
double X = rectangular.X();
double Y = rectangular.Y();
double Z = rectangular.Z();
double a = radius;
double b = a - flattening * a;
if ( Z < 0.)
b = -b;
if ( (X == 0.) && (Y == 0.) )
{
m_longitude.Set( 0. );
m_latitude.Set( 0. );
m_height = Z - b;
if ( m_height < 0. )
m_height = - m_height;
return;
}
else
m_longitude = ArcTan( Y, X );
if ( Z == 0.)
{
m_latitude.Set( 0. );
m_height.Set( r - radius );
return;
}
double r = std::sqrt( X * X + Y * Y );
double aSqr = a * a;
double bSqr = b * b;
double E = (b * Z - (aSqr - bSqr)) / (a * r);
double ESqr = E * E;
double F = (b * Z + (aSqr + bSqr)) / (a * r);
double P = (4./3.) * (E * F + 1.);
double Q = 2. * (ESqr - F * F);
double D = P * P * P + Q * Q;
double sqrtD = std::sqrt( D );
double v = std::pow( (sqrtD - Q), (1./3.) )
- std::pow( (sqrtD + Q), (1./3.) );
const double epsilon = 1.0;
if ( (std::fabs( Z ) < epsilon ) || (std::fabs( r ) < epsilon) )
v = - (v * v * v + 2. * Q) / (3. * P);
double G = 0.5 * (std::sqrt( ESqr + v ) + E);
double t = std::sqrt( G * G + (F - v * G) / (G + G - E) ) - G;
m_latitude = ArcTan( (a * (1. - t * t)), (2. * b * t) );
m_height = (r - a * t) * m_latitude.Cos( )
+ (Z - b) * m_latitude.Sin( );
}
void
Spherical::Set( const Point3D & rectangular )
{
if ( (rectangular.X() == 0.) && (rectangular.Y() == 0.) )
m_longitude.Set( 0. );
else
m_longitude = ArcTan( rectangular.Y(), rectangular.X() );
double x2y2 = rectangular.X() * rectangular.X()
+ rectangular.Y() * rectangular.Y();
double r = sqrt( x2y2 );
if ( (r == 0.) && (rectangular.Z() == 0.) )
m_latitude.Set( 0. );
else
m_latitude = ArcTan( rectangular.Z(), r );
m_distance = sqrt( x2y2 + rectangular.Z() * rectangular.Z() );
}
inline static
void ToPolar( int x, int y, DPoint center, double& r, double& theta )
{
double dx = x - center.x;
double dy = y - center.y;
r = Sqrt( dx*dx + dy*dy );
theta = ArcTan( dy, dx );
}
Angle
EclipticalLongitude( const Point3D & equatorialRect, Angle obliquity )
{
double x = equatorialRect.X();
double y = obliquity.Cos() * equatorialRect.Y()
+ obliquity.Sin() * equatorialRect.Z();
if ( (x == 0.) && (y == 0.) )
return Angle( 0. );
else
return ArcTan( y, x );
}
Ecliptical
EquatorialToEcliptical( const Equatorial & equatorial, Angle obliquity )
{
double sinRA = equatorial.RightAscension().Sin( );
double cosRA = equatorial.RightAscension().Cos( );
double sinDec = equatorial.Declination().Sin( );
double cosDec = equatorial.Declination().Cos( );
double tanDec = (cosDec == 0.) ? infinity : sinDec / cosDec;
double sinObl = obliquity.Sin( );
double cosObl = obliquity.Cos( );
Angle lng = ArcTan( sinRA * cosObl + tanDec * sinObl, cosRA );
lng.NormalizePositive( );
Angle lat = ArcSin( sinDec * cosObl - sinRA * cosDec * sinObl );
return Ecliptical( lng, lat, equatorial.Distance() );
}
Equatorial
EclipticalToEquatorial( const Ecliptical & ecliptical, Angle obliquity )
{
double sinLong = ecliptical.Longitude().Sin( );
double cosLong = ecliptical.Longitude().Cos( );
double sinLat = ecliptical.Latitude().Sin( );
double cosLat = ecliptical.Latitude().Cos( );
double tanLat = (cosLat == 0.) ? infinity : sinLat / cosLat;
double sinObl = obliquity.Sin( );
double cosObl = obliquity.Cos( );
Angle ra = ArcTan( sinLong * cosObl - tanLat * sinObl, cosLong );
ra.NormalizePositive( );
Angle dec = ArcSin( sinLat * cosObl + sinLong * cosLat * sinObl );
return Equatorial( ra, dec, ecliptical.Distance() );
}
Horizontal
EquatorialToHorizontal( const Equatorial & equatorial,
Angle localSiderealTime, Angle geographicLatitude )
{
Angle hourAngle = localSiderealTime - equatorial.RightAscension();
double sinHA = hourAngle.Sin( );
double cosHA = hourAngle.Cos( );
double sinDec = equatorial.Declination().Sin( );
double cosDec = equatorial.Declination().Cos( );
double tanDec = (cosDec == 0.) ? infinity : sinDec / cosDec;
double sinLat = geographicLatitude.Sin( );
double cosLat = geographicLatitude.Cos( );
Angle az = ArcTan( sinHA, cosHA * sinLat - tanDec * cosLat );
az += Angle( M_PI );
az.NormalizePositive( );
Angle alt = ArcSin( sinDec * sinLat + cosHA * cosDec * cosLat );
return Horizontal( az, alt, equatorial.Distance() );
}
Equatorial
HorizontalToEquatorial( const Horizontal & horizontal,
Angle localSiderealTime, Angle geographicLatitude )
{
Angle az = horizontal.Azimuth() - Angle( M_PI );
double sinAz = az.Sin( );
double cosAz = az.Cos( );
double sinAlt = horizontal.Altitude().Sin( );
double cosAlt = horizontal.Altitude().Cos( );
double tanAlt = (cosAlt == 0.) ? infinity : sinAlt / cosAlt;
double sinLat = geographicLatitude.Sin( );
double cosLat = geographicLatitude.Cos( );
// (Meeus has an error here, which I've corrected.)
Angle hourAngle = ArcTan( sinAz, cosAz * sinLat + tanAlt * cosLat );
Angle ra = localSiderealTime - hourAngle;
ra.NormalizePositive( );
Angle dec = ArcSin( sinAlt * sinLat - cosAz * cosAlt * cosLat );
return Equatorial( ra, dec, horizontal.Distance() );
}
Equatorial
GalacticToEquatorial( const Galactic & galactic )
{
Angle galNorthRAAdj( 192.25 - 180., Angle::Degree );
Angle galNorthDec( 27.4, Angle::Degree );
Angle longOffset( 33. + 90., Angle::Degree );
Angle adjLong = galactic.Longitude() - longOffset;
double sinAdjLong = adjLong.Sin( );
double cosAdjLong = adjLong.Cos( );
double sinLat = galactic.Latitude().Sin( );
double cosLat = galactic.Latitude().Cos( );
double tanLat = (cosLat == 0.) ? infinity : sinLat / cosLat;
double sinNDec = galNorthDec.Sin( );
double cosNDec = galNorthDec.Cos( );
Angle y = ArcTan( sinAdjLong, cosAdjLong * sinNDec - tanLat * cosNDec );
Angle ra = y + galNorthRAAdj;
ra.NormalizePositive( );
Angle dec = ArcSin( sinLat * sinNDec + cosAdjLong * cosLat * cosNDec );
return Equatorial( ra, dec, galactic.Distance() );
}
Galactic
EquatorialToGalactic( const Equatorial & equatorial )
{
Angle galNorthRA( 192.25, Angle::Degree );
Angle galNorthDec( 27.4, Angle::Degree );
Angle longOffset( 33. + 270., Angle::Degree );
Angle adjRA = galNorthRA - equatorial.RightAscension();
double sinAdjRA = adjRA.Sin( );
double cosAdjRA = adjRA.Cos( );
double sinDec = equatorial.Declination().Sin( );
double cosDec = equatorial.Declination().Cos( );
double tanDec = (cosDec == 0.) ? infinity : sinDec / cosDec;
double sinNDec = galNorthDec.Sin( );
double cosNDec = galNorthDec.Cos( );
Angle x = ArcTan( sinAdjRA, cosAdjRA * sinNDec - tanDec * cosNDec );
Angle lng = longOffset - x;
lng.NormalizePositive( );
Angle lat = ArcSin( sinDec * sinNDec + cosAdjRA * cosDec * cosNDec );
return Galactic( lng, lat, equatorial.Distance() );
}
Angle
RotationMatrix2<T>::GetAngle( ) const
{
return ArcTan( m_matrix( 1, 0 ), m_matrix( 0, 0 ) );
}
void Generate_Sphere(void)
{
/* first generate vertices */
{
int i,j;
VECTORCH *v = SphereVertex;
VECTORCH *o = OctantVertex;
Generate_SphereOctant();
/* north pole */
*v++ = *o;
for (i=0; ++i<=SPHERE_ORDER;)
{
o += i;
/* 1st Quadrant */
for (j=i; --j>=0; o++, v++)
{
*v = *o;
}
/* 2nd Quadrant */
for (j=i; --j>=0; o--, v++)
{
v->vx = -o->vx;
v->vy = o->vy;
v->vz = o->vz;
}
/* 3rd Quadrant */
for (j=i; --j>=0; o++, v++)
{
v->vx = -o->vx;
v->vy = -o->vy;
v->vz = o->vz;
}
/* 4th Quadrant */
for (j=i; --j>=0; o--, v++)
{
v->vx = o->vx;
v->vy = -o->vy;
v->vz = o->vz;
}
}
for (; --i>1;)
{
o -= i;
/* 5th Quadrant */
for (j=i; --j>0; o++, v++)
{
v->vx = o->vx;
v->vy = o->vy;
v->vz = -o->vz;
}
/* 6th Quadrant */
for (j=i; --j>0; o--, v++)
{
v->vx = -o->vx;
v->vy = o->vy;
v->vz = -o->vz;
}
/* 7th Quadrant */
for (j=i; --j>0; o++, v++)
{
v->vx = -o->vx;
v->vy = -o->vy;
v->vz = -o->vz;
}
/* 8th Quadrant */
for (j=i; --j>0; o--, v++)
{
v->vx = o->vx;
v->vy = -o->vy;
v->vz = -o->vz;
}
}
o--;
/* south pole */
v->vx = -o->vx;
v->vy = -o->vy;
v->vz = -o->vz;
}
/* now generate face data */
{
TRI_FACE *f = SphereFace;
int kv,kw,ko,kv0,kw0,i,j;
kv = 0, kw = 1;
for(i=0; i<SPHERE_ORDER; i++)
{
kv0 = kv, kw0 = kw;
for (ko=1; ko<=3; ko++)
{
for (j=i;; j--)
{
MakeFace(f,kv,kw,++kw);
if (j==0) break;
MakeFace(f,kv,kw,++kv);
}
}
for (j=i;; j--)
{
if (j==0)
{
MakeFace(f,kv0,kw,kw0);
kv++;
kw++;
break;
}
MakeFace(f,kv,kw,++kw);
if (j==1)
{
MakeFace(f,kv,kw,kv0);
}
else MakeFace(f,kv,kw,++kv);
}
}
for(; --i>=0;)
{
kv0=kv,kw0=kw;
for(ko=5; ko<=7; ko++)
{
for (j=i;; j--)
{
MakeFace(f,kv,kw,++kv);
if (j==0) break;
MakeFace(f,kv,kw,++kw);
}
}
for (j=i;; j--)
{
if (j==0)
{
MakeFace(f,kv,kw0,kv0);
kv++;
kw++;
break;
}
MakeFace(f,kv,kw,++kv);
if (j==1)
{
MakeFace(f,kv,kw,kw0);
}
else MakeFace(f,kv,kw,++kw);
}
}
}
{
int i;
VECTORCH *vSphere = SphereVertex;
for(i=0; i<SPHERE_VERTICES; i++,vSphere++)
{
// int radius = vSphere->vx*vSphere->vx+vSphere->vz*vSphere->vz;
// if (radius<16384) radius = 16384;
// SphereAtmosU[i] = DIV_FIXED(ArcCos(vSphere->vy)*32*128*8,radius);
SphereAtmosV[i] = ArcCos(vSphere->vy)*32*128*SPHERE_TEXTURE_WRAP;//*8;
SphereAtmosU[i] = ArcTan(vSphere->vz,vSphere->vx)*16*128*SPHERE_TEXTURE_WRAP;//*8;
}
}
}
