github.com/pavlo67/common@v0.5.3/common/geolib/trajectory.go (about) 1 package geolib 2 3 import ( 4 "fmt" 5 "math" 6 ) 7 8 type Trajectory []Point 9 10 type TrajectoryPoint struct { 11 Point 12 Bearing 13 } 14 15 func GetTrajectory(from Point, directions []Direction) Trajectory { 16 trajectory := make(Trajectory, len(directions)+1) 17 trajectory[0] = from 18 19 for i, direction := range directions { 20 to := from.PointAtDirection(direction) 21 trajectory[i+1], from = to, to 22 } 23 24 return trajectory 25 } 26 27 const onPoints = "on geolib.Trajectory.Points()" 28 29 func (t Trajectory) Points(velocity, fps float64) ([]TrajectoryPoint, error) { 30 if !(velocity > 0) || math.IsInf(velocity, 1) { 31 return nil, fmt.Errorf("wrong velocity: %f / "+onPoints, velocity) 32 } else if !(fps > 0) || math.IsInf(fps, 1) { 33 return nil, fmt.Errorf("wrong fps: %f / "+onPoints, velocity) 34 } else if len(t) < 1 { 35 return nil, nil 36 } else if len(t) == 1 { 37 return []TrajectoryPoint{{t[0], 0}}, nil 38 } 39 40 currentDirection := 0 41 currentDistance := t[0].DistanceTo(t[1]) 42 currentBearing := t[0].BearingTo(t[1]) 43 44 tp := TrajectoryPoint{t[0], currentBearing} 45 var passedDistance float64 46 47 trajectoryPoints := []TrajectoryPoint{tp} 48 49 piece := velocity / fps 50 51 for { 52 if passedDistance+piece >= currentDistance { 53 if currentDirection >= len(t)-2 { 54 return append( 55 trajectoryPoints, 56 TrajectoryPoint{tp.PointAtDirection(Direction{currentBearing, piece}), currentBearing}, 57 ), nil 58 } 59 60 passedDistance = passedDistance + piece - currentDistance 61 62 currentDirection++ 63 currentDistance = t[currentDirection].DistanceTo(t[currentDirection+1]) 64 currentBearing = t[currentDirection].BearingTo(t[currentDirection+1]) 65 66 if passedDistance < currentDistance { 67 tp = TrajectoryPoint{ 68 t[currentDirection].PointAtDirection(Direction{currentBearing, passedDistance}), 69 currentBearing, 70 } 71 } else { 72 // setting "temporary point" 73 passedDistance -= piece 74 75 tp = TrajectoryPoint{ 76 t[currentDirection+1].PointAtDirection(Direction{currentBearing + 180, passedDistance}), 77 currentBearing, 78 } 79 80 continue 81 } 82 83 } else { 84 passedDistance += piece 85 tp = TrajectoryPoint{ 86 tp.PointAtDirection(Direction{currentBearing, piece}), 87 currentBearing, 88 } 89 } 90 91 trajectoryPoints = append(trajectoryPoints, tp) 92 } 93 }