Golang math.Atan2函数代码示例

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
}