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  }