func getBoundingBox(rf Radiusfence) (x1, x2, y1, y2 float64) {
var lat1, lat2, lon1, lon2 float64
//Convert long,lat to rad
latRad := rf.p.Latitude * DegToRad
longRad := rf.p.Longitude * DegToRad
northMost := math.Asin(math.Sin(latRad)*math.Cos(rf.r/Radius) + math.Cos(latRad)*math.Sin(rf.r/Radius)*math.Cos(North))
southMost := math.Asin(math.Sin(latRad)*math.Cos(rf.r/Radius) + math.Cos(latRad)*math.Sin(rf.r/Radius)*math.Cos(South))
eastMost := longRad + math.Atan2(math.Sin(East)*math.Sin(rf.r/Radius)*math.Cos(latRad), math.Cos(rf.r/Radius)-math.Sin(latRad)*math.Sin(latRad))
westMost := longRad + math.Atan2(math.Sin(West)*math.Sin(rf.r/Radius)*math.Cos(latRad), math.Cos(rf.r/Radius)-math.Sin(latRad)*math.Sin(latRad))
if northMost > southMost {
lat1 = southMost
lat2 = northMost
} else {
lat1 = northMost
lat2 = southMost
}
if eastMost > westMost {
lon1 = westMost
lon2 = eastMost
} else {
lon1 = eastMost
lon2 = westMost
}
return lat1, lat2, lon1, lon2
}
// Returns the fraction of the segment that was visible
func (l *Los) TestSeg(seg linear.Seg2) float64 {
seg.P = seg.P.Sub(l.in.Pos)
seg.Q = seg.Q.Sub(l.in.Pos)
wrap := len(l.in.Buffer.ZBuffer)
a1 := math.Atan2(seg.P.Y, seg.P.X)
a2 := math.Atan2(seg.Q.Y, seg.Q.X)
if a1 > a2 {
a1, a2 = a2, a1
seg.P, seg.Q = seg.Q, seg.P
}
if a2-a1 > math.Pi {
a1, a2 = a2, a1
seg.P, seg.Q = seg.Q, seg.P
}
start := int(((a1 / (2 * math.Pi)) + 0.5) * float64(len(l.in.Buffer.ZBuffer)))
end := int(((a2 / (2 * math.Pi)) + 0.5) * float64(len(l.in.Buffer.ZBuffer)))
count := 0.0
visible := 0.0
for i := start % wrap; i != end%wrap; i = (i + 1) % wrap {
dist2 := float32(rays[i].Isect(seg).Mag2())
if dist2 < l.in.Buffer.ZBuffer[i] {
visible += 1.0
}
count += 1.0
}
return visible / count
}
// ToLatLon converts the Vector3d cartesian coordinates to
// latitude longitude according to the WGS84 datum.
// It returns a lat/lon coordinates struct.
func (v *Vector3d) ToLatLon(datum Datum) Coordinates {
aa := datum.a
bb := datum.b
e2 := (aa*aa - bb*bb) / (aa * aa) // 1st eccentricity squared
ε2 := (aa*aa - bb*bb) / (bb * bb) // 2nd eccentricity squared
p := math.Sqrt(v.x*v.x + v.y*v.y) // distance from minor axis
R := math.Sqrt(p*p + v.z*v.z) // polar radius
// parametric latitude
tanβ := (bb * v.z) / (aa * p) * (1 + ε2*b/R)
sinβ := tanβ / math.Sqrt(1+tanβ*tanβ)
cosβ := sinβ / tanβ
// geodetic latitude
φ := math.Atan2(v.z+ε2*bb*sinβ*sinβ*sinβ, p-e2*aa*cosβ*cosβ*cosβ)
// longitude
λ := math.Atan2(v.y, v.x)
coords := Coordinates{
Lat: toDegrees(φ),
Lon: toDegrees(λ),
}
return coords
}
func (f *Filter) madgwick(timestamp int64, values []float32) {
C.madgwick_update_array(
f.orientation, C.float(sampleFreq), C.float(beta),
(*_Ctype_float)(&values[0]))
// quaternion slice
o := f.orientation
q := []float32{
float32(o.q0),
float32(o.q1),
float32(o.q2),
float32(o.q3),
}
q1 := float64(o.q0)
q2 := float64(o.q1)
q3 := float64(o.q2)
q4 := float64(o.q3)
// euler angles in radians (madgwick 2010)
z := math.Atan2(2*q2*q3-2*q1*q4, 2*q1*q1+2*q2*q2-1) * rad2deg
y := -math.Asin(2*q2*q4+2*q1*q3) * rad2deg
x := math.Atan2(2*q3*q4-2*q1*q2, 2*q1*q1+2*q4*q4-1) * rad2deg
e := []float64{z, y, x}
if false {
log.Println("qtn", timestamp, q)
log.Println("eul", timestamp, e)
}
broadcast(timestamp, "qtn", q)
}
func newHalfedge(edge *Edge, LeftCell, RightCell *Cell) *Halfedge {
ret := &Halfedge{
Cell: LeftCell,
Edge: edge,
}
// 'angle' is a value to be used for properly sorting the
// halfsegments counterclockwise. By convention, we will
// use the angle of the line defined by the 'site to the left'
// to the 'site to the right'.
// However, border edges have no 'site to the right': thus we
// use the angle of line perpendicular to the halfsegment (the
// edge should have both end points defined in such case.)
if RightCell != nil {
ret.Angle = math.Atan2(RightCell.Site.Y-LeftCell.Site.Y, RightCell.Site.X-LeftCell.Site.X)
} else {
va := edge.Va
vb := edge.Vb
// rhill 2011-05-31: used to call GetStartpoint()/GetEndpoint(),
// but for performance purpose, these are expanded in place here.
if edge.LeftCell == LeftCell {
ret.Angle = math.Atan2(vb.X-va.X, va.Y-vb.Y)
} else {
ret.Angle = math.Atan2(va.X-vb.X, vb.Y-va.Y)
}
}
return ret
}
func (l *Los) DrawSeg(seg linear.Seg2, source string) {
seg.P = seg.P.Sub(l.in.Pos)
seg.Q = seg.Q.Sub(l.in.Pos)
wrap := len(l.in.Buffer.ZBuffer)
a1 := math.Atan2(seg.P.Y, seg.P.X)
a2 := math.Atan2(seg.Q.Y, seg.Q.X)
if a1 > a2 {
a1, a2 = a2, a1
seg.P, seg.Q = seg.Q, seg.P
}
if a2-a1 > math.Pi {
a1, a2 = a2, a1
seg.P, seg.Q = seg.Q, seg.P
}
start := int(((a1 / (2 * math.Pi)) + 0.5) * float64(len(l.in.Buffer.ZBuffer)))
end := int(((a2 / (2 * math.Pi)) + 0.5) * float64(len(l.in.Buffer.ZBuffer)))
for i := start % wrap; i != end%wrap; i = (i + 1) % wrap {
dist2 := float32(rays[i].Isect(seg).Mag2())
// dist = rays[i].Isect(seg).Mag2()
if dist2 < l.in.Buffer.ZBuffer[i] {
l.in.Buffer.ZBuffer[i] = dist2
l.in.Buffer.SBuffer[i] = source
}
}
}
// Convert Cartesian coordinates to polar.
// The reference ellipsoid is copied verbatim to the result.
// The resulting polar coordinates are in decimal degrees.
// Inspired by http://www.movable-type.co.uk/scripts/latlong-convert-coords.html
func CartesianToPolar(pt *CartPoint) *PolarCoord {
var gc PolarCoord
el := pt.El
esq := (el.a*el.a - el.b*el.b) / (el.a * el.a)
p := math.Hypot(pt.X, pt.Y)
lat := math.Atan2(pt.Z, p*(1-esq))
lat0 := 2.0 * math.Pi
precision := 4.0 / el.a
var v float64
for math.Abs(lat-lat0) > precision {
v = el.a / math.Sqrt(1-esq*math.Pow(math.Sin(lat), 2))
lat0 = lat
lat = math.Atan2(pt.Z+esq*v*math.Sin(lat), p)
}
gc.Height = p/math.Cos(lat) - v
gc.Latitude = radtodeg(lat)
gc.Longitude = radtodeg(math.Atan2(pt.Y, pt.X))
gc.El = el
return &gc
}
// Move the arm to the point (x,y,z) following the path of an arc whose centre is at (i,j,k).
//
// The distance between the current position and (i,j,k) must equal that between (x,y,z) and (i,j,k).
func (s *Staubli) ArcCenter(x, y, z, i, j, k, direction float64) error {
// TODO rewrite this nicer. This was copypaster'd from the V+ code. It can be a lot nicer.
i += s.cur.x
j += s.cur.y
k += s.cur.z
startAngle := math.Atan2(s.cur.y-j, s.cur.x-i)
endAngle := math.Atan2(y-j, x-i)
rX := (s.cur.x - i)
rY := (s.cur.y - j)
radius := math.Sqrt(rX*rX + rY*rY)
arcLen := math.Abs((endAngle - startAngle) / direction)
zStep := (z - s.cur.z) / arcLen
for a := 0.0; a < arcLen; a++ {
angle := direction*a + startAngle
x = radius*math.Cos(angle) + i
y = radius*math.Sin(angle) + j
z = s.cur.z + a*zStep
err := s.MoveStraight(x, y, z)
if err != nil {
return err
}
}
return nil
}
// Position returns observed equatorial coordinates of a planet at a given time.
//
// Argument p must be a valid V87Planet object for the observed planet.
// Argument earth must be a valid V87Planet object for Earth.
//
// Results are right ascension and declination, α and δ in radians.
func Position(p, earth *pp.V87Planet, jde float64) (α, δ float64) {
L0, B0, R0 := earth.Position(jde)
L, B, R := p.Position(jde)
sB0, cB0 := math.Sincos(B0)
sL0, cL0 := math.Sincos(L0)
sB, cB := math.Sincos(B)
sL, cL := math.Sincos(L)
x := R*cB*cL - R0*cB0*cL0
y := R*cB*sL - R0*cB0*sL0
z := R*sB - R0*sB0
{
Δ := math.Sqrt(x*x + y*y + z*z) // (33.4) p. 224
τ := base.LightTime(Δ)
// repeating with jde-τ
L, B, R = p.Position(jde - τ)
sB, cB = math.Sincos(B)
sL, cL = math.Sincos(L)
x = R*cB*cL - R0*cB0*cL0
y = R*cB*sL - R0*cB0*sL0
z = R*sB - R0*sB0
}
λ := math.Atan2(y, x) // (33.1) p. 223
β := math.Atan2(z, math.Hypot(x, y)) // (33.2) p. 223
Δλ, Δβ := apparent.EclipticAberration(λ, β, jde)
λ, β = pp.ToFK5(λ+Δλ, β+Δβ, jde)
Δψ, Δε := nutation.Nutation(jde)
λ += Δψ
sε, cε := math.Sincos(nutation.MeanObliquity(jde) + Δε)
return coord.EclToEq(λ, β, sε, cε)
// Meeus gives a formula for elongation but doesn't spell out how to
// obtaion term λ0 and doesn't give an example solution.
}
// compute the centroid of a polygon set
// using a spherical co-ordinate system
func getCentroid(ps geo.PointSet) *geo.Point {
X := 0.0
Y := 0.0
Z := 0.0
var toRad = math.Pi / 180
var fromRad = 180 / math.Pi
for _, point := range ps {
var lon = point[0] * toRad
var lat = point[1] * toRad
X += math.Cos(lat) * math.Cos(lon)
Y += math.Cos(lat) * math.Sin(lon)
Z += math.Sin(lat)
}
numPoints := float64(len(ps))
X = X / numPoints
Y = Y / numPoints
Z = Z / numPoints
var lon = math.Atan2(Y, X)
var hyp = math.Sqrt(X*X + Y*Y)
var lat = math.Atan2(Z, hyp)
var centroid = geo.NewPoint(lon*fromRad, lat*fromRad)
return centroid
}
func (d *MPU6050) calculatePitchAndRoll() {
accel := d.accel_reading
// fmt.Printf("accel: %f, %f, %f\n", accel.x, accel.y, accel.z)
// Accel.
p1 := math.Atan2(float64(accel.y), dist(accel.x, accel.z))
p1_deg := p1 * (180 / math.Pi)
r1 := math.Atan2(float64(accel.x), dist(accel.y, accel.z))
r1_deg := -r1 * (180 / math.Pi)
// Gyro.
p2 := float64(d.gyro_reading.x)
r2 := float64(d.gyro_reading.y) // Backwards?
// "Noise filter".
ft := float64(0.98)
sample_period := float64(1 / 2000.0)
d.pitch = float64(ft*(sample_period*p2+d.pitch) + (1-ft)*p1_deg)
d.roll = float64((ft*(sample_period*r2+d.roll) + (1-ft)*r1_deg))
d.pitch_history = append(d.pitch_history, d.pitch)
d.roll_history = append(d.roll_history, d.roll)
}
func NewStdElems(r, r_dot util.Vector3D) (s StdOrbElems) {
r0 := math.Sqrt(r.Dot(r))
v02 := r_dot.Dot(r_dot)
s.A = 2/r0 - v02*EarthMuInv
s.A = 1 / s.A
h := r.Cross(r_dot)
fmt.Println(h, r, r_dot)
// fmt.Println(EarthMu, EarthMuInv)
fmt.Println(r0, math.Sqrt(v02))
c := r_dot.Cross(h).Add(r.Scale(-1 * EarthMu / r0))
fmt.Println(c, r.Scale(-1*EarthMu/r0), r_dot.Cross(h))
// fmt.Println(math.Sqrt(c.Dot(c)) / EarthMu)
s.Ecc = math.Sqrt(c.Dot(c)) / EarthMu
h_mag := math.Sqrt(h.Dot(h))
ie := c.Scale(1 / (EarthMu * s.Ecc))
ih := h.Scale(1 / h_mag)
ip := ih.Cross(ie)
s.LongAscend = math.Atan2(ih[0], -ih[1])
s.Inc = math.Acos(ih[2])
s.ArgOfPeri = math.Atan2(ie[2], ip[2])
sig0 := r.Dot(r_dot) / math.Sqrt(EarthMu)
E0 := math.Atan2(sig0/math.Sqrt(s.A), 1-r0/s.A)
s.MeanAnom0 = E0 - s.Ecc*math.Sin(E0)
return
}
// bounce ball travelling in direction av off line b.
// return the new unit vector.
func boing(av realPoint, ln line) realPoint {
f := ln.p1.Sub(ln.p0)
d := math.Atan2(float64(f.Y), float64(f.X))*2 - math.Atan2(av.y, av.x)
p := realPoint{math.Cos(d), math.Sin(d)}
return p
}
func (c *Cone) IntersectP(r RayBase) bool {
// Transform _Ray_ to object space
ray := r.Transform(c.worldToObject)
// Compute quadratic cone coefficients
k := c.radius / c.height
k = k * k
A := ray.Dir().X*ray.Dir().X + ray.Dir().Y*ray.Dir().Y - k*ray.Dir().Z*ray.Dir().Z
B := 2.0 * (ray.Dir().X*ray.Origin().X + ray.Dir().Y*ray.Origin().Y - k*ray.Dir().Z*(ray.Origin().Z-c.height))
C := ray.Origin().X*ray.Origin().X + ray.Origin().Y*ray.Origin().Y - k*(ray.Origin().Z-c.height)*(ray.Origin().Z-c.height)
// Solve quadratic equation for _t_ values
var t0, t1 float64
var ok bool
if ok, t0, t1 = Quadratic(A, B, C); !ok {
return false
}
// Compute intersection distance along ray
if t0 > ray.Maxt() || t1 < ray.Mint() {
return false
}
thit := t0
if t0 < ray.Mint() {
thit = t1
if thit > ray.Maxt() {
return false
}
}
// Compute cone inverse mapping
phit := ray.PointAt(thit)
phi := math.Atan2(phit.Y, phit.X)
if phi < 0.0 {
phi += 2.0 * math.Pi
}
// Test cone intersection against clipping parameters
if phit.Z < 0 || phit.Z > c.height || phi > c.phiMax {
if thit == t1 {
return false
}
thit = t1
if t1 > ray.Maxt() {
return false
}
// Compute cone inverse mapping
phit = ray.PointAt(thit)
phi = math.Atan2(phit.Y, phit.X)
if phi < 0.0 {
phi += 2.0 * math.Pi
}
if phit.Z < 0 || phit.Z > c.height || phi > c.phiMax {
return false
}
}
return true
}
// Angle returns the angle between great circles defined by three points.
//
// Coordinates may be right ascensions and declinations or longitudes and
// latitudes. If r1, d1, r2, d2 defines one line and r2, d2, r3, d3 defines
// another, the result is the angle between the two lines.
//
// Algorithm by Meeus.
func Angle(r1, d1, r2, d2, r3, d3 float64) float64 {
sd2, cd2 := math.Sincos(d2)
sr21, cr21 := math.Sincos(r2 - r1)
sr32, cr32 := math.Sincos(r3 - r2)
C1 := math.Atan2(sr21, cd2*math.Tan(d1)-sd2*cr21)
C2 := math.Atan2(sr32, cd2*math.Tan(d3)-sd2*cr32)
return C1 + C2
}
// Angle returns the angle between great circles defined by three points.
//
// Coordinates may be right ascensions and declinations or longitudes and
// latitudes. If r1, d1, r2, d2 defines one line and r2, d2, r3, d3 defines
// another, the result is the angle between the two lines.
//
// Algorithm by Meeus.
func Angle(r1, d1, r2, d2, r3, d3 unit.Angle) unit.Angle {
sd2, cd2 := d2.Sincos()
sr21, cr21 := (r2 - r1).Sincos()
sr32, cr32 := (r3 - r2).Sincos()
C1 := math.Atan2(sr21, cd2*d1.Tan()-sd2*cr21)
C2 := math.Atan2(sr32, cd2*d3.Tan()-sd2*cr32)
return unit.Angle(C1 + C2)
}
// LatLon returns the geographic coordinates of a point.
func (p Geo) LatLon() (lat, lon float64) {
c := p.c()
φ := math.Atan2(c[2], math.Hypot(c[0], c[1]))
λ := math.Atan2(c[1], c[0])
lat = φ * (180 / math.Pi)
lon = λ * (180 / math.Pi)
return
}
func (p *Paraboloid) IntersectP(r RayBase) bool {
// Transform _Ray_ to object space
ray := r.Transform(p.worldToObject)
// Compute quadratic paraboloid coefficients
k := p.Zmax / (p.radius * p.radius)
A := k * (ray.Dir().X*ray.Dir().X + ray.Dir().Y*ray.Dir().Y)
B := 2*k*(ray.Dir().X*ray.Origin().X+ray.Dir().Y*ray.Origin().Y) - ray.Dir().Z
C := k*(ray.Origin().X*ray.Origin().X+ray.Origin().Y*ray.Origin().Y) - ray.Origin().Z
// Solve quadratic equation for _t_ values
var t0, t1 float64
var ok bool
if ok, t0, t1 = Quadratic(A, B, C); !ok {
return false
}
// Compute intersection distance along ray
if t0 > ray.Maxt() || t1 < ray.Mint() {
return false
}
thit := t0
if t0 < ray.Mint() {
thit = t1
if thit > ray.Maxt() {
return false
}
}
// Compute paraboloid inverse mapping
phit := ray.PointAt(thit)
phi := math.Atan2(phit.Y, phit.X)
if phi < 0.0 {
phi += 2.0 * math.Pi
}
// Test paraboloid intersection against clipping parameters
if phit.Z < p.Zmin || phit.Z > p.Zmax || phi > p.phiMax {
if thit == t1 {
return false
}
thit = t1
if t1 > ray.Maxt() {
return false
}
// Compute paraboloid inverse mapping
phit = ray.PointAt(thit)
phi = math.Atan2(phit.Y, phit.X)
if phi < 0.0 {
phi += 2.0 * math.Pi
}
if phit.Z < p.Zmin || phit.Z > p.Zmax || phi > p.phiMax {
return false
}
}
return true
}
func (c *Cylinder) IntersectP(r RayBase) bool {
// Transform _Ray_ to object space
ray := r.Transform(c.worldToObject)
// Compute quadratic cylinder coefficients
A := ray.Dir().X*ray.Dir().X + ray.Dir().Y*ray.Dir().Y
B := 2.0 * (ray.Dir().X*ray.Origin().X + ray.Dir().Y*ray.Origin().Y)
C := ray.Origin().X*ray.Origin().X + ray.Origin().Y*ray.Origin().Y - c.radius*c.radius
// Solve quadratic equation for _t_ values
var t0, t1 float64
var ok bool
if ok, t0, t1 = Quadratic(A, B, C); !ok {
return false
}
// Compute intersection distance along ray
if t0 > ray.Maxt() || t1 < ray.Mint() {
return false
}
thit := t0
if t0 < ray.Mint() {
thit = t1
if thit > ray.Maxt() {
return false
}
}
// Compute cylinder hit point and $\phi$
phit := ray.PointAt(thit)
phi := math.Atan2(phit.Y, phit.X)
if phi < 0.0 {
phi += 2.0 * math.Pi
}
// Test cylinder intersection against clipping parameters
if phit.Z < c.Zmin || phit.Z > c.Zmax || phi > c.phiMax {
if thit == t1 {
return false
}
thit = t1
if t1 > ray.Maxt() {
return false
}
// Compute cylinder hit point and $\phi$
phit = ray.PointAt(thit)
phi = math.Atan2(phit.Y, phit.X)
if phi < 0.0 {
phi += 2.0 * math.Pi
}
if phit.Z < c.Zmin || phit.Z > c.Zmax || phi > c.phiMax {
return false
}
}
return true
}
// cl splits the work into two closures.
func cl(jde float64, earth, saturn *pp.V87Planet) (f1 func() (ΔU, B float64),
f2 func() (Bʹ, P, aEdge, bEdge float64)) {
const p = math.Pi / 180
var i, Ω float64
var l0, b0, R float64
Δ := 9.
var λ, β float64
var si, ci, sβ, cβ, sB float64
var sbʹ, cbʹ, slʹΩ, clʹΩ float64
f1 = func() (ΔU, B float64) {
// (45.1), p. 318
T := base.J2000Century(jde)
i = base.Horner(T, 28.075216*p, -.012998*p, .000004*p)
Ω = base.Horner(T, 169.50847*p, 1.394681*p, .000412*p)
// Step 2.
l0, b0, R = earth.Position(jde)
l0, b0 = pp.ToFK5(l0, b0, jde)
sl0, cl0 := math.Sincos(l0)
sb0 := math.Sin(b0)
// Steps 3, 4.
var l, b, r, x, y, z float64
f := func() {
τ := base.LightTime(Δ)
l, b, r = saturn.Position(jde - τ)
l, b = pp.ToFK5(l, b, jde)
sl, cl := math.Sincos(l)
sb, cb := math.Sincos(b)
x = r*cb*cl - R*cl0
y = r*cb*sl - R*sl0
z = r*sb - R*sb0
Δ = math.Sqrt(x*x + y*y + z*z)
}
f()
f()
// Step 5.
λ = math.Atan2(y, x)
β = math.Atan(z / math.Hypot(x, y))
// First part of step 6.
si, ci = math.Sincos(i)
sβ, cβ = math.Sincos(β)
sB = si*cβ*math.Sin(λ-Ω) - ci*sβ
B = math.Asin(sB) // return value
// Step 7.
N := 113.6655*p + .8771*p*T
lʹ := l - .01759*p/r
bʹ := b - .000764*p*math.Cos(l-N)/r
// Setup for steps 8, 9.
sbʹ, cbʹ = math.Sincos(bʹ)
slʹΩ, clʹΩ = math.Sincos(lʹ - Ω)
// Step 9.
sλΩ, cλΩ := math.Sincos(λ - Ω)
U1 := math.Atan2(si*sbʹ+ci*cbʹ*slʹΩ, cbʹ*clʹΩ)
U2 := math.Atan2(si*sβ+ci*cβ*sλΩ, cβ*cλΩ)
ΔU = math.Abs(U1 - U2) // return value
return
}
// Grena3 calculates topocentric solar position following algorithm number 3
// described in Grena, 'Five new algorithms for the computation of sun position
// from 2010 to 2110', Solar Energy 86 (2012) pp. 1323-1337.
func Grena3(date time.Time,
latitudeDegrees float64, longitudeDegrees float64,
deltaTSeconds float64,
pressureHPa float64, temperatureCelsius float64) (azimuthDegrees, zenithDegrees float64) {
t := calcT(date)
tE := t + 1.1574e-5*deltaTSeconds
omegaAtE := 0.0172019715 * tE
lambda := -1.388803 + 1.720279216e-2*tE + 3.3366e-2*math.Sin(omegaAtE-0.06172) +
3.53e-4*math.Sin(2.0*omegaAtE-0.1163)
epsilon := 4.089567e-1 - 6.19e-9*tE
sLambda := math.Sin(lambda)
cLambda := math.Cos(lambda)
sEpsilon := math.Sin(epsilon)
cEpsilon := math.Sqrt(1.0 - sEpsilon*sEpsilon)
alpha := math.Atan2(sLambda*cEpsilon, cLambda)
if alpha < 0 {
alpha = alpha + 2*math.Pi
}
delta := math.Asin(sLambda * sEpsilon)
h := 1.7528311 + 6.300388099*t + toRad(longitudeDegrees) - alpha
h = math.Mod((h+math.Pi), (2*math.Pi)) - math.Pi
if h < -math.Pi {
h = h + 2*math.Pi
}
sPhi := math.Sin(toRad(latitudeDegrees))
cPhi := math.Sqrt((1 - sPhi*sPhi))
sDelta := math.Sin(delta)
cDelta := math.Sqrt(1 - sDelta*sDelta)
sH := math.Sin(h)
cH := math.Cos(h)
sEpsilon0 := sPhi*sDelta + cPhi*cDelta*cH
eP := math.Asin(sEpsilon0) - 4.26e-5*math.Sqrt(1.0-sEpsilon0*sEpsilon0)
gamma := math.Atan2(sH, cH*sPhi-sDelta*cPhi/cDelta)
deltaRe := 0.0
if eP > 0 && pressureHPa > 0.0 && pressureHPa < 3000.0 &&
temperatureCelsius > -273 && temperatureCelsius < 273 {
deltaRe = (0.08422 * (pressureHPa / 1000)) / ((273.0 + temperatureCelsius) *
math.Tan(eP+0.003138/(eP+0.08919)))
}
z := math.Pi/2 - eP - deltaRe
azimuthDegrees = convertAzimuth(gamma)
zenithDegrees = toDeg(z)
return
}
func (s *Sphere) Color(p Vector) Color {
if s.material.Texture == nil {
return s.material.Color
}
u := math.Atan2(p.Z, p.X)
v := math.Atan2(p.Y, Vector{p.X, 0, p.Z}.Length())
u = (u + math.Pi) / (2 * math.Pi)
v = 1 - (v+math.Pi/2)/math.Pi
return s.material.Texture.Sample(u, v)
}
/* Decompose a 3x3 rotation matrix into euler angles (degrees)
* http://nghiaho.com/?page_id=846 */
func FromOdeRotation(mat3 ode.Matrix3) mgl.Vec3 {
x := math.Atan2(mat3[2][1], mat3[2][2])
y := math.Atan2(-mat3[2][0], math.Sqrt(math.Pow(mat3[2][1], 2)+math.Pow(mat3[2][2], 2)))
z := math.Atan2(mat3[1][0], mat3[0][0])
return mgl.Vec3{
float32(x * rad2deg),
float32(y * rad2deg),
float32(z * rad2deg),
}
}
// angle in radians between the two vectors
func (v1 Vector2D) Angle(v2 Vector2D) float64 {
a := math.Atan2(v2.Y, v2.X) - math.Atan2(v1.Y, v1.X)
if a > math.Pi {
return a - 2.0*math.Pi
}
if a < -math.Pi {
return a + 2.0*math.Pi
}
return a
}
// intersectsLatEdge reports if the edge AB intersects the given edge of constant
// latitude. Requires the points to have unit length.
func intersectsLatEdge(a, b Point, lat s1.Angle, lng s1.Interval) bool {
// Unfortunately, lines of constant latitude are curves on
// the sphere. They can intersect a straight edge in 0, 1, or 2 points.
// First, compute the normal to the plane AB that points vaguely north.
z := a.PointCross(b)
if z.Z < 0 {
z = Point{z.Mul(-1)}
}
// Extend this to an orthonormal frame (x,y,z) where x is the direction
// where the great circle through AB achieves its maximium latitude.
y := z.PointCross(PointFromCoords(0, 0, 1))
x := y.Cross(z.Vector)
// Compute the angle "theta" from the x-axis (in the x-y plane defined
// above) where the great circle intersects the given line of latitude.
sinLat := math.Sin(float64(lat))
if math.Abs(sinLat) >= x.Z {
// The great circle does not reach the given latitude.
return false
}
cosTheta := sinLat / x.Z
sinTheta := math.Sqrt(1 - cosTheta*cosTheta)
theta := math.Atan2(sinTheta, cosTheta)
// The candidate intersection points are located +/- theta in the x-y
// plane. For an intersection to be valid, we need to check that the
// intersection point is contained in the interior of the edge AB and
// also that it is contained within the given longitude interval "lng".
// Compute the range of theta values spanned by the edge AB.
abTheta := s1.IntervalFromEndpoints(
math.Atan2(a.Dot(y.Vector), a.Dot(x)),
math.Atan2(b.Dot(y.Vector), b.Dot(x)))
if abTheta.Contains(theta) {
// Check if the intersection point is also in the given lng interval.
isect := x.Mul(cosTheta).Add(y.Mul(sinTheta))
if lng.Contains(math.Atan2(isect.Y, isect.X)) {
return true
}
}
if abTheta.Contains(-theta) {
// Check if the other intersection point is also in the given lng interval.
isect := x.Mul(cosTheta).Sub(y.Mul(sinTheta))
if lng.Contains(math.Atan2(isect.Y, isect.X)) {
return true
}
}
return false
}
// ReduceElements reduces orbital elements of a solar system body from one
// equinox to another.
//
// This function is described in chapter 24, but is located in this
// package so it can be a method of EclipticPrecessor.
func (p *EclipticPrecessor) ReduceElements(eFrom, eTo *elementequinox.Elements) *elementequinox.Elements {
ψ := p.π + p.p
si, ci := math.Sincos(eFrom.Inc)
snp, cnp := math.Sincos(eFrom.Node - p.π)
// (24.1) p. 159
eTo.Inc = math.Acos(ci*p.cη + si*p.sη*cnp)
// (24.2) p. 159
eTo.Node = math.Atan2(si*snp, p.cη*si*cnp-p.sη*ci) + ψ
// (24.3) p. 159
eTo.Peri = math.Atan2(-p.sη*snp, si*p.cη-ci*p.sη*cnp) + eFrom.Peri
return eTo
}
// ExtractEulerAngles extracts the rotation part of the matrix as Euler angle rotation values.
func (mat *T) ExtractEulerAngles() (yHead, xPitch, zRoll float64) {
xPitch = math.Asin(mat[1][2])
f12 := math.Abs(mat[1][2])
if f12 > (1.0-0.0001) && f12 < (1.0+0.0001) { // f12 == 1.0
yHead = 0.0
zRoll = math.Atan2(mat[0][1], mat[0][0])
} else {
yHead = math.Atan2(-mat[0][2], mat[2][2])
zRoll = math.Atan2(-mat[1][0], mat[1][1])
}
return yHead, xPitch, zRoll
}
// Topocentric returns topocentric positions including parallax.
//
// Arguments α, δ are geocentric right ascension and declination in radians.
// Δ is distance to the observed object in AU. ρsφʹ, ρcφʹ are parallax
// constants (see package globe.) L is geographic longitude of the observer,
// jde is time of observation.
//
// Results are observed topocentric ra and dec in radians.
func Topocentric(α, δ, Δ, ρsφʹ, ρcφʹ, L, jde float64) (αʹ, δʹ float64) {
π := Horizontal(Δ)
θ0 := base.Time(sidereal.Apparent(jde)).Rad()
H := base.PMod(θ0-L-α, 2*math.Pi)
sπ := math.Sin(π)
sH, cH := math.Sincos(H)
sδ, cδ := math.Sincos(δ)
Δα := math.Atan2(-ρcφʹ*sπ*sH, cδ-ρcφʹ*sπ*cH) // (40.2) p. 279
αʹ = α + Δα
δʹ = math.Atan2((sδ-ρsφʹ*sπ)*math.Cos(Δα), cδ-ρcφʹ*sπ*cH) // (40.3) p. 279
return
}
// ReduceB1950ToJ2000 reduces orbital elements of a solar system body from
// equinox B1950 in the FK4 system to equinox J2000 in the FK5 system.
func ReduceB1950FK4ToJ2000FK5(eFrom, eTo *Elements) *Elements {
W := _L + eFrom.Node
si, ci := eFrom.Inc.Sincos()
sJ, cJ := _J.Sincos()
sW, cW := W.Sincos()
eTo.Inc = unit.Angle(math.Acos(ci*cJ - si*sJ*cW))
eTo.Node = (unit.Angle(math.Atan2(si*sW, ci*sJ+si*cJ*cW)) -
_Lp).Mod1()
eTo.Peri = (eFrom.Peri +
unit.Angle(math.Atan2(sJ*sW, si*cJ+ci*sJ*cW))).Mod1()
return eTo
}
// Topocentric returns topocentric positions including parallax.
//
// Arguments α, δ are geocentric right ascension and declination in radians.
// Δ is distance to the observed object in AU. ρsφʹ, ρcφʹ are parallax
// constants (see package globe.) L is geographic longitude of the observer,
// jde is time of observation.
//
// Results are observed topocentric ra and dec in radians.
func Topocentric(α unit.RA, δ unit.Angle, Δ, ρsφʹ, ρcφʹ float64, L unit.Angle, jde float64) (αʹ unit.RA, δʹ unit.Angle) {
π := Horizontal(Δ)
θ0 := sidereal.Apparent(jde)
H := (θ0.Angle() - L - unit.Angle(α)).Mod1()
sπ := π.Sin()
sH, cH := H.Sincos()
sδ, cδ := δ.Sincos()
// (40.2) p. 279
Δα := unit.HourAngle(math.Atan2(-ρcφʹ*sπ*sH, cδ-ρcφʹ*sπ*cH))
αʹ = α.Add(Δα)
// (40.3) p. 279
δʹ = unit.Angle(math.Atan2((sδ-ρsφʹ*sπ)*Δα.Cos(), cδ-ρcφʹ*sπ*cH))
return
}