diff --git a/goverdrive/Makefile b/goverdrive/Makefile new file mode 100644 index 0000000..f209199 --- /dev/null +++ b/goverdrive/Makefile @@ -0,0 +1,65 @@ +GOFILES=$(shell find . -name '*.go') +GOBINDIR=$(shell echo $(GOPATH) | awk -F: '{ print $$NF }')/bin + +.PHONY: build clean save-deps restore-deps test + +.DEFAULT_GOAL := all + +ALL_EXAMPLES=mover drive sidetap zoneshapes +ALL_GAMES=chicken connect fourmation + + +ifdef GOBINDIR +clean: + rm -f $(ALL_EXAMPLES) $(ALL_GAMES) +endif + + +###################################################################### +# TESTS +###################################################################### + +phystest: + go test -v -timeout 1m -race github.com/anki/goverdrive/phys/... + +tracktest: + go test -v -timeout 1m -race github.com/anki/goverdrive/robo/track/... + +robotest: + go test -v -timeout 1m -race github.com/anki/goverdrive/robo + +test: phystest tracktest robotest + + +###################################################################### +# EXAMPLES +###################################################################### + +mover: $(GOFILES) + go build github.com/anki/goverdrive/games/example/mover/ + +drive: $(GOFILES) + go build github.com/anki/goverdrive/games/example/drive/ + +sidetap: $(GOFILES) + go build github.com/anki/goverdrive/games/example/sidetap/ + +zoneshapes: $(GOFILES) + go build github.com/anki/goverdrive/games/example/zoneshapes/ + +examples: $(ALL_EXAMPLES) + + +###################################################################### +# GAMES +###################################################################### + +chicken: $(GOFILES) + go build github.com/anki/goverdrive/games/gwenz/chicken/ + +connect: $(GOFILES) + go build github.com/anki/goverdrive/games/gwenz/connect/ + +fourmation: $(GOFILES) + go build github.com/anki/goverdrive/games/gwenz/fourmation/ + diff --git a/goverdrive/README.md b/goverdrive/README.md new file mode 100644 index 0000000..6ab1854 --- /dev/null +++ b/goverdrive/README.md @@ -0,0 +1,103 @@ +# goverdrive + +A lightweight, standalone OverDrive simulator. + +- Written in Go +- Designed to be a tool for Game Design on top of the OverDrive robotics platform +- Created by Gabriel Wenz (`gwenz@anki.com`) + +![goverdrive Screenshot](doc/Screenshot.png) + + +## Features +- Any fixed-width track made up of arbitrary straight and curved road pieces +- Any number of vehicles +- Control driving speed, offset from road center, and driving direction of each vehicle +- Perfect knowledge of vehicle position and state at all times +- Collision detection +- Flexible vehicle lights geometry and control +- Programs compile in ~1 second and launch instantly +- Build working game prototypes with <500 lines of code + +### Not Supported / Not Present +- Multi-loop tracks +- Fancy track pieces like Intersection and Jump Piece +- AI of any kind +- Path planning +- Default game logic, weapons, etc + + +## Requirements + +- Go +- OpenGL development libraries. See the documentation for [Pixel](https://godoc.org/github.com/faiface/pixel). + + +## Installation Instructions + +Background reading, for new Go developers: + [Go Code Organization](https://golang.org/doc/code.html#Organization) + +1. Install Go +2. Setup the project work area and download source files + +For example: `~/proj/go/goverdrive/` +``` +$ cd ~/proj/go/ +$ mkdir goverdrive +$ cp ~/Downloads/proj_setup.sh goverdrive/proj_setup.sh +$ chmod +x goverdrive/proj_setup.sh +$ cd goverdrive +$ ./proj_setup.sh +``` + +3. Build example programs +``` +$ make examples +go build github.com/anki/goverdrive/games/example/mover/ +go build github.com/anki/goverdrive/games/example/drive/ +go build github.com/anki/goverdrive/games/example/sidetap/ +go build github.com/anki/goverdrive/games/example/zoneshapes/ +``` + +4. Run an example of your own configuration: +``` +$ ./drive -h +Usage of ./drive: + -ins + Display instructions at the start of each game phase + -mb uint + Message board height, expressed as integer number of pixels. Can be 0. (default 200) + -t string + Track name or modular track string (default "Capsule") + -tmaxcofs float + Track max center offset, from road center + -twidth float + Track width, in Meters (default 0.2) + -v string + List of vehicles, using two-letter abberviations; eg "gs sk" for Groundshock and Skull (default "gs") + -w string + Window size, expressed as integer pixels WIDTHxHEIGHT (default "1200x850") +``` +or this canned one: +``` +$ ./drive -t loopback -v "gs th" +``` + + +## Example Programs + +See [games/example/](games/example/README.md) + + +## Enhancment Ideas + +* Helper module: `gameutil/shapes/animation`. Examples to consider: + * Explosion + * Weapon + * Persistent "jet flame" from vehicle + * Rotating coins or mines +* Generalize user input in gameloop.RunGamePhase, eg use empty interface => enable batch-mode sims +* gameutil/vehtraj - track trajectory a vehicle; then view, analyze, write to file, etc +* Force-based robotics simulator + diff --git a/goverdrive/doc/GameLoop.md b/goverdrive/doc/GameLoop.md new file mode 100644 index 0000000..aadfa02 --- /dev/null +++ b/goverdrive/doc/GameLoop.md @@ -0,0 +1,54 @@ +This document presents a high-level view of the seqeuence of game and +simulation "ticks" or "updates" that make goverdrive go. + +``` +RunGameLoop(gamePhase) { + // No sim time passes in gamePhase.Start(). Game state can be + // initialized, and vehicles can be repositioned, eg to simulate a + // race lineup. + gamePhase.Start(rsys) + + done = false + while !done { + // robo.System.Tick() updates the position and state of all vehicles, + // based on their current state and commands from the game phase. + // This is is the part of the program where sim time advances. + // Auxiliary robotics tasks, such as updating the vehicle lights and + // collision detection, also happen as part of the robotics simulation + // tick. + rsys.Tick() + + // gamePhase.Update() may do any of the following: + // - Process user input + // - Issue new vehicle commands + // - Update the game state + // Update() returns all of the extra objects that need to visualized, + // such as game shapes, track regions, and message board text. + // + // Worth noting: + // - GamePhase.Update() may issue commands to vehicles, but it does + // NOT directly move or change the state of the vehicles. Vehicle + // movement and state updates are performed by the robotics + // simulator. + // - EXCEPTION: Vehicle.Reposition() DOES directly change the + // position of the vehicle. + done = gamePhase.Update(...) + + // wait for remainder of real-time tick to finish + + // Window.Update() draws the track, vehicles, and other game + // objects to the display. It also gathers new input (eg from + // the keyboard) for the next gaemPhase.Update(). + Window.Update() + + // Momentary Pause + while IsPressed(Backspace (ie delete) key) { + // no sim or game updates; only monitor key presses + } + } + + // No sim time passes in gamePhase.Stop(). Final game state and ranking + // can be determined. + gamePhase.Stop(rsys) +} +``` diff --git a/goverdrive/doc/Representations.md b/goverdrive/doc/Representations.md new file mode 100644 index 0000000..7db26e8 --- /dev/null +++ b/goverdrive/doc/Representations.md @@ -0,0 +1,180 @@ +# PHYSICAL UNITS + +Fundamental physical units are defined in the `phys` package. + +In all representations: + +- Unit of length is Meters, since real physical objects are being modeled +- Unit of angle is Radians (not Degrees), since that's how math works +- Other units, such as mass, also use metric + + +# THE WORLD + +The "world" is a coordinate space with two dimensions. Superstring +Theory be damned. + +Depending on the needs of the moment, Cartesian or Polar coordinates +may be used. + +![World Coordinates](WorldCoord.png) + + +## Cartesian + +- X is the primary axis +- Y is the secondary axis +- Z, if it were present, would be the tertiary axis, and would follow the Right Hand Rule + +## Polar + +- R is the radius (Meters) +- Phi is the angle (Radians) +- Converting between Cartesian and Polar coordinates is easy + +## Pose + +A pose in two-dimentional space adds an angle, Theta, to specify the +orientation of an object. In the case of vehicles, this is the +direction they are facing. + +- Theta is in Radians + + +# THE TRACK + +The track is a non-Euclidean 2D coordinate space. Track points are +specified using a distance offset and center offset, and these values +"bend" to match the shape of the track. + +![Track Coordinates](TrackCoord4.png) + + +## Trackwise and Counter-Trackwise + +Like a clock, the track has a natural "forward" driving direction, +which is called Trackwise. For modular OverDrive tracks, this is +determined by the orientation of the Start Piece. + +Counter-Trackwise means opposite of the track's natural forward +driving direction. + + +## Dofs +Dofs is the primary axis for track coordinates + +- Dofs is short for "Distance Offset" +- Measures the distance from the finish line along road center, in trackwise direction +- Unit is Meters +- Dofs values are cyclic, like the angle for a polar coordinate system +- Dofs will never exceed the track length (at road center) + + +## Cofs + +Cofs is the secondary axis for track coordinates + +- Cofs is short for "Center Offset" +- Measures displacement from road center +- Unit is Meters +- The road center is at Cofs=0 +- In trackwise driving direction, Cofs>0 is left of road center + + +## Vofs +Vofs is NOT present; if it were present, would be the tertiary axis + +- Vofs is short for "Vertical Offset" +- Would measure the height of the track off the ground +- Would follow the Right Hand Rule + + +## Track Pose + +Similar to the Cartesian "World" coordinate space, Track Pose adds an +angle, DAngle, to a vehicle's Track Position. However, the Track Pose +angle is not absolute. Instead, it is a DELTA amount from the +Trackwise driving direction at that point in the track. + +- DAngle is in Radians +- A vehicle that is driving Trackwise with no active Cofs changes will have DAngle=0 +- A vehilce that is driving Counter-Trackwise with no active Cofs changes will have DAngle=Pi + + +# DRIVE FRAME-OF-REFERENCE + +The "Drive" frame-of-reference takes the reference object's pose into +account when determining the Dofs and Cofs values. (In most cases, the +object is a vehicle that is driving on the track, hence the name +"Drive".) Positions are still measured w.r.t. fixed track landmarks: +the Finish Line and road center. + +When the object is facing Trackwise, Dofs and Cofs meaning is as +described above. + +When the object is facing Counter-Trackwise, however, the Drive +frame-of-reference has: + +1. Sign of Cofs (`+` or `-`) is reversed + - Cofs>0 means LEFT of road center in object's frame-of-reference, + which is RIGHT of road center in the Trackwise direction. + +2. Dofs is the distance from the Finish Line in the "driving" + direction of the object, + - This is the same as the distance REMAINING in the Trackwise direction. + - Dofs values in Drive frame-of-reference will still be in the range + (0 < Dofs < Track.CenLen()) + +This may all seem a bit confusing, but in practice it's easy enough to +work with. In most cases, the Drive frame-of-reference simplifies code +that works with vehicles, because that code can be agnostic to the +actual driving direction of the vehicle. + + +## DriveDelta Measurements + +DriveDelta measures Dofs/Cofs distances relative to an object itself, +not relative to fixed track landmarks such as the Finish Line and road +center. For example, if Vehicle B is exactly 0.2 meters behind Vehicle +A, then DriveDeltaDofs from Vehicle A to Vehicle B would be -0.2. + +DriveDelta measurements are handy in various game situations, such as +maintaining a formation relationship between vehicles, or checking if +one vehicle has passed another vehicle. + + +# COMBINED WORLD AND TRACK + +## Track and World Coordinates Overlay + +The midpoint of the track's Finish Line is always placed at the origin +of the World, ie (X=0, Y=0). The trackwise driving direction starts in +the +X direction. + + +## Converting Between World and Track Coordinates + +Converting a point in Track coordinate space to World coordinate space + +- Straightforward +- 1-to-1 mapping + +Converting a point in World coordinate space to Track coordinate space + +- Not always straightforward, due to overlapping track pieces +- Not a 1-to-1 mapping + + +## Visual Examples + +The images below show several vehicles on the track. For each vehicle, +the Cartesian and Track coordinates are printed in the message board +area of the display. + +- The World origin is always the start piece, with +X to the right. +- The start piece is indicated with a lime green triangle. +- Vehicle names are abbeviated (gs=Groundshock, sk=Skull, etc) + +![Track Coordinates 1](TrackCoord1.png) +![Track Coordinates 2](TrackCoord2.png) +![Track Coordinates 3](TrackCoord3.png) diff --git a/goverdrive/doc/Screenshot.png b/goverdrive/doc/Screenshot.png new file mode 100644 index 0000000..dcc7c36 Binary files /dev/null and b/goverdrive/doc/Screenshot.png differ diff --git a/goverdrive/doc/TrackCoord1.png b/goverdrive/doc/TrackCoord1.png new file mode 100644 index 0000000..89137dc Binary files /dev/null and b/goverdrive/doc/TrackCoord1.png differ diff --git a/goverdrive/doc/TrackCoord2.png b/goverdrive/doc/TrackCoord2.png new file mode 100644 index 0000000..f49c94c Binary files /dev/null and b/goverdrive/doc/TrackCoord2.png differ diff --git a/goverdrive/doc/TrackCoord3.png b/goverdrive/doc/TrackCoord3.png new file mode 100644 index 0000000..9106085 Binary files /dev/null and b/goverdrive/doc/TrackCoord3.png differ diff --git a/goverdrive/doc/TrackCoord4.png b/goverdrive/doc/TrackCoord4.png new file mode 100644 index 0000000..bca225f Binary files /dev/null and b/goverdrive/doc/TrackCoord4.png differ diff --git a/goverdrive/doc/WorldCoord.png b/goverdrive/doc/WorldCoord.png new file mode 100644 index 0000000..1b68a9f Binary files /dev/null and b/goverdrive/doc/WorldCoord.png differ diff --git a/goverdrive/engine/cliconfig.go b/goverdrive/engine/cliconfig.go new file mode 100644 index 0000000..034eb44 --- /dev/null +++ b/goverdrive/engine/cliconfig.go @@ -0,0 +1,133 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com +// +// cliconfig.go provides a means to configure many parts of the game (track, +// vehicles, etc) in using standard command-line arguments. Unless there is a +// very good reason not to, this is THE way to configure these parts of the +// game. + +package engine + +import ( + "flag" + "fmt" + "strings" + + "github.com/faiface/pixel" + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" +) + +// CLIGameConfig is the game's configuration, based on command-line values +type CLIGameConfig struct { + trk *track.Track + vehs []robo.Vehicle + win *pixelgl.Window + mbHeight uint + showInstr bool +} + +// NewCLIGameConfig parses command-line arguments and creates a game +// configuration based on their values. +func NewCLIGameConfig(title string, lightSpec light.Spec) *CLIGameConfig { + var gc CLIGameConfig + + winFlag /*******/ := flag.String("w", "1200x850", "Window size, expressed as integer pixels WIDTHxHEIGHT") + mbFlag /********/ := flag.Uint("mb", 200, "Message board height, expressed as integer number of pixels. Can be 0.") + tWidthFlag /****/ := flag.Float64("twidth", 0.20, "Track width, in Meters") + tMaxCofsFlag /**/ := flag.Float64("tmaxcofs", 0.0, "Track max center offset, from road center") + trackFlag /*****/ := flag.String("t", "Capsule", "Track name or modular track string") + vehsFlag /******/ := flag.String("v", "gs", "List of vehicles, using two-letter abberviations; eg \"gs sk\" for Groundshock and Skull") + insFlag /*******/ := flag.Bool("ins", false, "Display instructions at the start of each game phase") + flag.Parse() + + // parse the window size + var winWidth, winHeight uint + if n, werr := fmt.Sscanf(*winFlag, "%dx%d", &winWidth, &winHeight); (werr != nil) || (n != 2) { + panic(fmt.Sprintf("win=\"%s\" could not be parsed as WxH pixels", *winFlag)) + } + + // message board height + gc.mbHeight = *mbFlag + if gc.mbHeight > (winHeight / 2) { + panic(fmt.Sprintf("Message board height=%v is too big, relative to window height=%v", gc.mbHeight, winHeight)) + } + + // track width + twidth := phys.Meters(*tWidthFlag) + if (twidth < 0.001) || (twidth > 2.0) { + panic(fmt.Sprintf("Track width=%v is not reasonable", twidth)) + } + + // game instructions + gc.showInstr = *insFlag + + // create the track + tMaxCofs := phys.Meters(*tMaxCofsFlag) + if *trackFlag != "" { + gc.trk, _ = track.NewModularTrack(twidth, tMaxCofs, *trackFlag) + if gc.trk == nil { + gc.trk, _ = track.NewStarterKitTrack(twidth, tMaxCofs, *trackFlag) + } + if gc.trk == nil { + gc.trk, _ = track.NewCustomTrack(twidth, tMaxCofs, *trackFlag) + } + } + if gc.trk == nil { + fmt.Printf("Supported starter kit tracks:\n %s\n", track.StarterKitTrackNames("\n ")) + fmt.Printf("Supported custom tracks:\n %s\n", track.CustomTrackNames("\n ")) + panic("A valid track is required to proceed!") + } + + // create the vehicles + vehStrList := strings.Split(*vehsFlag, " ") + gc.vehs = make([]robo.Vehicle, 0) + for _, vs := range vehStrList { + gc.vehs = append(gc.vehs, *robo.NewVehicle(robo.VehType(vs), lightSpec, gc.trk.CenLen())) + } + + // create the window + winCfg := pixelgl.WindowConfig{ + Title: title, + Bounds: pixel.R(0, 0, float64(winWidth), float64(winHeight)), + VSync: true, + } + var werr error + gc.win, werr = pixelgl.NewWindow(winCfg) + if werr != nil { + panic(werr) + } + + return &gc +} + +// Track returns a pointer to the track that was created +func (gc *CLIGameConfig) Track() *track.Track { + return gc.trk +} + +// Vehicles returns a pointer to the vehicles that were created +func (gc *CLIGameConfig) Vehicles() *[]robo.Vehicle { + return &gc.vehs +} + +// Window returns a pointer to the window that was created +func (gc *CLIGameConfig) Window() *pixelgl.Window { + return gc.win +} + +// MsgBoardPixHeight returns the number of vertical pixels that should be +// dedicated to the message board. +func (gc *CLIGameConfig) MsgBoardPixHeight() uint { + return gc.mbHeight +} + +// ShowInstructions returns true if instructions should be displayed before the +// start of each game phase. +func (gc *CLIGameConfig) ShowInstructions() bool { + return gc.showInstr +} diff --git a/goverdrive/engine/gameloop.go b/goverdrive/engine/gameloop.go new file mode 100644 index 0000000..00284fc --- /dev/null +++ b/goverdrive/engine/gameloop.go @@ -0,0 +1,152 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package engine has the reusable game engine core, such as the game loop. +package engine + +import ( + "fmt" + "golang.org/x/image/colornames" + "math" + "sort" + "time" + + "github.com/faiface/pixel" + "github.com/faiface/pixel/pixelgl" + "github.com/faiface/pixel/text" + "golang.org/x/image/font/basicfont" + + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/viz" +) + +const ( + // TODO(gwenz): It's unclear what range of values works for + // roboTicksPerGameTick, and whether there is any need for a game developer to + // change it. + roboTicksPerGameTick uint = 2 + + mbPaddingPixX = 20 + mbPaddingPixY = 40 +) + +// GamePhaseVizConfig is a wrapper for all of the visualization configuration, +// such as the window, and how much of the window is occupied by the Message +// Board. +type GamePhaseVizConfig struct { + ShowInstr bool + MsgBoardPixHeight uint // pixels + WorldViz viz.WorldViz + Window *pixelgl.Window + atlas *text.Atlas +} + +// RunGameLoop is the core loop that drives the game. It runs one game phase +// from start to finish, with the supplied visualization config and robotics +// system. To run without vizualization or UI, set the window to nil. +// +// RunGameLoop includes: +// - Robotics simulation +// - User input +// - Rendering the world, and displaying it to a window +func RunGameLoop(vizCfg GamePhaseVizConfig, rsys *robo.System, phase GamePhase) { + fmt.Printf("track.CenLen()=%v, track.MinCorner()=%v, track.MaxCorner=%v\n", rsys.Track.CenLen(), rsys.Track.MinCorner(), rsys.Track.MaxCorner()) + //fmt.Printf("winBounds.Min=%v, winBounds.Max=%v\n", vizCfg.Window.Bounds().Min, vizCfg.Window.Bounds().Max) + + vizCfg.atlas = text.NewAtlas(basicfont.Face7x13, text.ASCII) + vizCfg.Window.SetSmooth(true) // less pixelated rendering + + phase.Start(rsys) + + if vizCfg.ShowInstr { + // before starting the game, display instructions on the message board + vizObj := EmptyGamePhaseVizObjects() + vizObj.MBText = phase.InstructionText(rsys) + "\n<<>>" + drawToWindow(vizCfg, rsys, vizObj) + fps := time.Tick(time.Second / 20) + for !vizCfg.Window.JustReleased(pixelgl.KeySpace) { + vizCfg.Window.Update() + <-fps + } + } + + var gameDeltaT time.Duration = time.Duration(uint64(roboTicksPerGameTick)*uint64(rsys.SimDeltaT())) * time.Nanosecond + gameDelay := time.After(gameDeltaT) + done := false + for !done && !vizCfg.Window.Closed() { + // Robotics simulation + for i := uint(0); i < roboTicksPerGameTick; i++ { + rsys.Tick() + } + + // Game logic + isDone, vizObj := phase.Update(rsys, vizCfg.Window) + done = isDone + + // Display and inputs + if vizCfg.Window != nil { + <-gameDelay + gameDelay = time.After(gameDeltaT) + drawToWindow(vizCfg, rsys, vizObj) + vizCfg.Window.Update() // display and inputs + + // momentary pause (while key is pressed) + if vizCfg.Window.JustPressed(pixelgl.KeyBackspace) { + fps := time.Tick(time.Second / 20) + for !vizCfg.Window.JustReleased(pixelgl.KeyBackspace) { + vizCfg.Window.Update() + <-fps + } + } + } + } + + phase.Stop(rsys) + + if done { + // Show the final vehicle ranking on the Message Board + + vizObj := EmptyGamePhaseVizObjects() + rstr := "" + rankings := VehRankingSorter{phase.VehRankings()} + sort.Sort(&rankings) // in-place + for _, r := range rankings.Rankings { + rstr += fmt.Sprintf("[%s] %s\n", rsys.Vehicles[r.VehId].Type(), r.String()) + } + vizObj.MBText = rstr + "\nDONE. Press SPACE BAR to continue.." + drawToWindow(vizCfg, rsys, vizObj) + fps := time.Tick(time.Second / 20) + for !vizCfg.Window.JustReleased(pixelgl.KeySpace) { + vizCfg.Window.Update() + <-fps + } + } +} + +func drawToWindow(vizCfg GamePhaseVizConfig, rsys *robo.System, vizObj GamePhaseVizObjects) { + canvas := vizCfg.WorldViz.RenderAll(&rsys.Track, vizObj.Regions, &rsys.Vehicles, vizObj.Shapes) + + // TODO(gwenz): Encapsulate window/canvas/text/etc into package viz, so + // that gameloop does not directly depend on visualization implementation? + + // stretch the canvas to fit the window + // (leave room at the bottom for the message board) + scaleFactor := math.Min( + vizCfg.Window.Bounds().W()/canvas.Bounds().W(), + (vizCfg.Window.Bounds().H()-float64(vizCfg.MsgBoardPixHeight))/canvas.Bounds().H()) + + winBounds := vizCfg.Window.Bounds() + winBounds.Max.Y += float64(vizCfg.MsgBoardPixHeight) + vizCfg.Window.Clear(colornames.Black) + vizCfg.Window.SetMatrix(pixel.IM.Scaled(pixel.ZV, scaleFactor).Moved(winBounds.Center())) + canvas.Draw(vizCfg.Window, pixel.IM) + + mbPos := pixel.Vec{ + X: mbPaddingPixX - (winBounds.Center().X / scaleFactor), + Y: (-winBounds.Center().Y+float64(vizCfg.MsgBoardPixHeight))/scaleFactor - mbPaddingPixY, + } + txt := text.New(pixel.V(0, 0), vizCfg.atlas) + txt.Color = colornames.Lightgrey + txt.WriteString(vizObj.MBText) + txt.Draw(vizCfg.Window, pixel.IM.Scaled(pixel.ZV, 1.4/scaleFactor).Moved(mbPos)) +} diff --git a/goverdrive/engine/gamephase.go b/goverdrive/engine/gamephase.go new file mode 100644 index 0000000..187db9c --- /dev/null +++ b/goverdrive/engine/gamephase.go @@ -0,0 +1,99 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package engine + +import ( + "fmt" + + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/viz" + "github.com/faiface/pixel/pixelgl" +) + +// VehRanking is for reporting the rank of a vehicle, compared to other +// vehicles. The precise meaning of ranking is intentionally vague. +type VehRanking struct { + VehId int // handle to the vehicle + Rank int // (typical) 1=1st place, 2=2nd place, etc + ScoreString string // arbitrary string representation of a vehicle's score (eg "10 points") +} + +func (vr *VehRanking) String() string { + return fmt.Sprintf("VehId %d: Rank: %d Score: %s", vr.VehId, vr.Rank, vr.ScoreString) +} + +// VehRankingSorter is a wrapper to use sort.Interface +type VehRankingSorter struct { + Rankings []VehRanking +} + +// Len implements function needed for sort.Interface +func (vrs *VehRankingSorter) Len() int { + return len(vrs.Rankings) +} + +// Less implements comparison needed for sort.Interface +func (vrs *VehRankingSorter) Less(i, j int) bool { + return vrs.Rankings[i].Rank < vrs.Rankings[j].Rank +} + +// Swap implements function needed for sort.Interface +func (vrs *VehRankingSorter) Swap(i, j int) { + vrs.Rankings[i], vrs.Rankings[j] = vrs.Rankings[j], vrs.Rankings[i] +} + +////////////////////////////////////////////////////////////////////// + +// GamePhaseVizObjects is a wrapper with all of the game-specific objects +// (beyond the built-in track and vehicles) from the GamePhase, which need to +// visualized at a particular moment in time. +type GamePhaseVizObjects struct { + Regions *[]*viz.TrackRegion + Shapes *[]*viz.GameShape + MBText string // message board +} + +// EmptyGamePhaseVizObjects returns a GamePhaseVizObjects that has been properly +// initialized with empty slices. +func EmptyGamePhaseVizObjects() GamePhaseVizObjects { + emptyReg := make([]*viz.TrackRegion, 0) + emptyShp := make([]*viz.GameShape, 0) + return GamePhaseVizObjects{ + Regions: &emptyReg, + Shapes: &emptyShp, + MBText: "", + } +} + +////////////////////////////////////////////////////////////////////// + +// GamePhase is the meat of gameplay. It drives interactions between the track +// and vehicles. A full "game" has one or more game phases. +// - The same track is used for all game phases +// - The same set of vehicles is used for all game phases +// - The state of the vehicles can be preserved between game phases +// - The vehicles have a ranking +type GamePhase interface { + // InstructionText returns a text instruction string for the game phase. It + // can be multi-line. + InstructionText(rsys *robo.System) string + + // Start does any one-time setup needed for the game phase. No time passes, + // but the vehicle states may be changed, eg to reposition for a fake lineup. + Start(rsys *robo.System) + + // Update is the "tick" to run the game logic. + // - The robotics system is available to query and command; it includes the time + // - User input can be retrieved from the window + // - Game-specific objects are returned for visualization + // - When the game phase is done, true is returned + Update(rsys *robo.System, win *pixelgl.Window) (bool, GamePhaseVizObjects) + + // Stop terminates the game phase, and computes final vehicle rankings. No + // time passes. + Stop(rsys *robo.System) + + // VehRankings returns the ranking of each vehicle. + VehRankings() []VehRanking +} diff --git a/goverdrive/games/example/README.md b/goverdrive/games/example/README.md new file mode 100644 index 0000000..5c030b4 --- /dev/null +++ b/goverdrive/games/example/README.md @@ -0,0 +1,34 @@ +# EXAMPLES + +## mover + +Example `mover` manually moves vehicles around the track cars by +respositioning. There is no actual driving or game play. + +The coordinates for each vehicle are reported on the message board, +which makes this a good program to interactively explore Track and +Cartesian coordinate space representations. + + +## drive + +Example `drive` drives any number of vehicles on a track. + +One vehicle is selected at a time, and the selected vehicle's track +direction, speed, and lane position can be changed. There is no actual +game play. + + +## zoneshapes + +Example `zoneshapes` shows usage of track regions and game shapes. The +track regions are drawn on the track, and each region triggers the +appearance of one or more game shapes when a vehicle enters it. There +is no actual game play. + + +## sidetap + +Example game `sidetap` is a very simple game that demonstrates vehicle +collisions and non-standard vehicle lights. Win the game by colliding +to knock out your opponent's side lights. diff --git a/goverdrive/games/example/drive/game.go b/goverdrive/games/example/drive/game.go new file mode 100644 index 0000000..c5bc770 --- /dev/null +++ b/goverdrive/games/example/drive/game.go @@ -0,0 +1,130 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + "golang.org/x/image/colornames" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/gameutil/lapmetrics" + "github.com/anki/goverdrive/gameutil/vehlights" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" + "github.com/anki/goverdrive/viz" +) + +const ( + minDspd = 0.3 + maxDspd = 1.5 +) + +// DriveGamePhase does simple driving for a set of vehicles. +type DriveGamePhase struct { + numVeh int + curVeh int + lapMetrics lapmetrics.LapMetrics + lapText string +} + +func (gp *DriveGamePhase) InstructionText(rys *robo.System) string { + return `SPACE BAR: Select which vehicle to control +LEFT/RIGHT ARROW KEYS: Change horizontal offset +UP/DOWN ARROW KEYS: Accelerate and decelerate +RIGHT SHIFT KEY: U-turn +` +} + +func (gp *DriveGamePhase) Start(rsys *robo.System) { + gp.lapMetrics = *lapmetrics.New(rsys.Now(), &rsys.Vehicles, true, true) + for i, _ := range rsys.Vehicles { + lineupPoint := track.Point{ + Dofs: rsys.Track.NormalizeDofs(-0.2 * phys.Meters(i)), + Cofs: 0} + rsys.Vehicles[i].Reposition(track.Pose{Point: lineupPoint, DAngle: 0}) + rsys.Vehicles[i].SetCmdDriveDspd(0.4, 1.0) + } + gp.numVeh = len(rsys.Vehicles) + gp.curVeh = 0 +} + +func (gp *DriveGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *DriveGamePhase) VehRankings() []engine.VehRanking { + rankings := make([]engine.VehRanking, gp.numVeh) + for v := 0; v < gp.numVeh; v++ { + rankings[v] = engine.VehRanking{VehId: v, Rank: v, ScoreString: "0"} + } + return rankings +} + +func (gp *DriveGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + veh := &rsys.Vehicles[gp.curVeh] + + if win.JustPressed(pixelgl.KeySpace) { + // advance control to next vehicle + gp.curVeh = ((gp.curVeh + 1) % gp.numVeh) + } + + dspd := veh.CmdDriveDspd() + if win.JustPressed(pixelgl.KeyUp) { + frames := []light.Frame{light.Frame{Color: colornames.Lime, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "guns", frames, 1) + dspd += 0.1 + if dspd > maxDspd { + dspd = maxDspd + } + veh.SetCmdDriveDspd(dspd, 0.4) + } + if win.JustPressed(pixelgl.KeyDown) { + frames := []light.Frame{light.Frame{Color: colornames.Red, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "tail", frames, 1) + dspd -= 0.1 + if dspd < minDspd { + dspd = minDspd + } + veh.SetCmdDriveDspd(dspd, 0.4) + } + if win.JustPressed(pixelgl.KeyRightShift) { + veh.CmdUturn(robo.DefUturnRadius) + } + + cofs := veh.CmdDriveCofs() + dCofs := phys.Meters(0) + if win.JustPressed(pixelgl.KeyLeft) { + dCofs = +0.025 + } + if win.JustPressed(pixelgl.KeyRight) { + dCofs = -0.025 + } + veh.SetCmdDriveCofs(cofs+dCofs, 0.1) + + // circle around the controlled vehicle + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameCirc(gp.curVeh, phys.Point{X: 0, Y: 0}, 0.05, colornames.White, 0.004)) + + // speedometer light + clr := vehlights.SpeedometerColor(vehlights.DefSpeedometerColors, veh.CurDriveDspd()) + veh.Lights().Set("top", clr) + + // lap counts + gp.lapMetrics.Update(rsys.Now(), &rsys.Track, &rsys.Vehicles) + for v := range rsys.Vehicles { + newlaps := gp.lapMetrics.NewCompletedLapInfo(v) + for _, li := range newlaps { + gp.lapText = fmt.Sprintf("Veh %d lap completed: %s\n", v, li.String()) + gp.lapText + } + } + + // message board text + vizObj.MBText = gp.InstructionText(rsys) + "\n" + gp.lapText + + return false, vizObj +} diff --git a/goverdrive/games/example/drive/main.go b/goverdrive/games/example/drive/main.go new file mode 100644 index 0000000..6ac1247 --- /dev/null +++ b/goverdrive/games/example/drive/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("Drive (goverdrive)", light.Gen2Spec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &DriveGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/games/example/mover/game.go b/goverdrive/games/example/mover/game.go new file mode 100644 index 0000000..cc067e9 --- /dev/null +++ b/goverdrive/games/example/mover/game.go @@ -0,0 +1,113 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + "golang.org/x/image/colornames" + "math" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/track" + "github.com/anki/goverdrive/viz" +) + +const ( + dDofs = 0.10 + dCofs = 0.02 +) + +// MoverGamePhase does simple driving for a set of vehicles. +type MoverGamePhase struct { + numVeh int + curVeh int +} + +func (gp *MoverGamePhase) InstructionText(rys *robo.System) string { + return `SPACE BAR: Select which vehicle to move +LEFT/RIGHT ARROW KEYS: Change center offset +UP/DOWN ARROW KEYS: Change distance offset +RIGHT SHIFT KEY: U-turn +` +} + +func (gp *MoverGamePhase) Start(rsys *robo.System) { + for i, _ := range rsys.Vehicles { + rsys.Vehicles[i].Reposition(track.Pose{Point: track.Point{Dofs: 0, Cofs: 0}, DAngle: 0}) + rsys.Vehicles[i].SetCmdDriveCofs(0, 0) + rsys.Vehicles[i].SetCmdDriveDspd(0, 0) + } + gp.numVeh = len(rsys.Vehicles) + gp.curVeh = 0 +} + +func (gp *MoverGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *MoverGamePhase) VehRankings() []engine.VehRanking { + rankings := make([]engine.VehRanking, gp.numVeh) + for v := 0; v < gp.numVeh; v++ { + rankings = append(rankings, engine.VehRanking{VehId: v, Rank: v, ScoreString: "0"}) + } + return rankings +} + +func (gp *MoverGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + veh := &rsys.Vehicles[gp.curVeh] + + if win.JustPressed(pixelgl.KeySpace) { + // advance control to next vehicle + gp.curVeh = ((gp.curVeh + 1) % gp.numVeh) + } + + tpose := veh.CurTrackPose() + if win.JustPressed(pixelgl.KeyRightShift) { + tpose.DAngle = phys.NormalizeRadians(tpose.DAngle + math.Pi) + } + if win.JustPressed(pixelgl.KeyUp) { + tpose.Dofs = rsys.Track.NormalizeDofs(tpose.Dofs + dDofs) + } + if win.JustPressed(pixelgl.KeyDown) { + tpose.Dofs = rsys.Track.NormalizeDofs(tpose.Dofs - dDofs) + } + if win.JustPressed(pixelgl.KeyLeft) { + tpose.Cofs += dCofs + } + if win.JustPressed(pixelgl.KeyRight) { + tpose.Cofs -= dCofs + } + veh.Reposition(tpose) + + // circle in the controlled vehicle + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameCirc(gp.curVeh, phys.Point{X: 0, Y: 0}, 0.01, colornames.White, 0)) + + // message board text + for _, veh2 := range (*rsys).Vehicles { + tpose := veh2.CurTrackPose() + pose := rsys.Track.ToPose(tpose) + vizObj.MBText += fmt.Sprintf("%s: Dofs=%.3f Cofs=%+.3f X=%+.3f Y=%+.3f Th=%+.5f\n", + veh2.Type(), tpose.Dofs, tpose.Cofs, pose.X, pose.Y, pose.Theta) + } + if len(rsys.Vehicles) > 1 { + frVeh := &rsys.Vehicles[0] + toVeh := &rsys.Vehicles[1] + vizObj.MBText += fmt.Sprintf("%s-->%s: ", frVeh.Type(), toVeh.Type()) + vizObj.MBText += fmt.Sprintf("DofsDist=%.3f ", rsys.Track.DofsDist(frVeh.CurTrackPose().Dofs, toVeh.CurTrackPose().Dofs)) + vizObj.MBText += fmt.Sprintf("DrDofsDist=%.3f ", rsys.Track.DriveDofsDist(frVeh.CurTrackPose(), toVeh.CurTrackPose().Dofs)) + vizObj.MBText += fmt.Sprintf("DrDeltaDofs=%.3f ", rsys.Track.DriveDeltaDofs(frVeh.CurTrackPose(), toVeh.CurTrackPose().Dofs)) + vizObj.MBText += fmt.Sprintf("DrDeltaCofs=%.3f ", rsys.Track.DriveDeltaCofs(frVeh.CurTrackPose(), toVeh.CurTrackPose().Cofs)) + vizObj.MBText += fmt.Sprintf("DrDist=%.3f ", rsys.Track.DriveDist(frVeh.CurTrackPose(), toVeh.CurTrackPose().Dofs)) + vizObj.MBText += fmt.Sprintf("DrDeltaDist=%.3f ", rsys.Track.DriveDeltaDist(frVeh.CurTrackPose(), toVeh.CurTrackPose().Dofs)) + vizObj.MBText += "\n" + } + vizObj.MBText += "\n" + gp.InstructionText(rsys) + + return false, vizObj +} diff --git a/goverdrive/games/example/mover/main.go b/goverdrive/games/example/mover/main.go new file mode 100644 index 0000000..adf4839 --- /dev/null +++ b/goverdrive/games/example/mover/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("Mover (goverdrive)", light.Gen2Spec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &MoverGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/games/example/sidetap/game.go b/goverdrive/games/example/sidetap/game.go new file mode 100644 index 0000000..2013ec6 --- /dev/null +++ b/goverdrive/games/example/sidetap/game.go @@ -0,0 +1,231 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + "golang.org/x/image/colornames" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" + "github.com/anki/goverdrive/viz" +) + +const ( + minDspd = 0.3 + maxDspd = 1.2 + + gameAccel = 0 + gameDecel = 1 + gameCofsL = 2 + gameCofsR = 3 + gameUturn = 4 +) + +type buttonMapType map[int]pixelgl.Button + +var buttonMap [2]buttonMapType + +// BumperCarsGamePhase does simple driving for a set of vehicles. +type BumperCarsGamePhase struct { + numVeh int + hitPointsL [2]int + hitPointsR [2]int +} + +func (gp *BumperCarsGamePhase) InstructionText(rys *robo.System) string { + return `Requires exactly two vehicles. +To win: Knock out all of your opponent's side lights, using collisions. + +ACTION VEHICLE 0 VEHILCE 1 +Change center offset Left A Key Arrow Key Left +Change center offset Right D Key Arrow Key Right +Accelerate W Key Arrow Key Up +Brake S Key Arrow Key Down +U-turn Left Shift Key Right Shift Key +` +} + +func (gp *BumperCarsGamePhase) Start(rsys *robo.System) { + buttonMap = [2]buttonMapType{ + buttonMapType{ // player 0 + gameAccel: pixelgl.KeyW, + gameDecel: pixelgl.KeyS, + gameCofsL: pixelgl.KeyA, + gameCofsR: pixelgl.KeyD, + gameUturn: pixelgl.KeyLeftShift, + }, + buttonMapType{ // player 1 + gameAccel: pixelgl.KeyUp, + gameDecel: pixelgl.KeyDown, + gameCofsL: pixelgl.KeyLeft, + gameCofsR: pixelgl.KeyRight, + gameUturn: pixelgl.KeyRightShift, + }, + } + + gp.numVeh = len(rsys.Vehicles) + if gp.numVeh != 2 { + panic("BumperCarsGamePhase requires exactly 2 vehicles") + } + for i, _ := range rsys.Vehicles { + lineupPoint := track.Point{ + Dofs: rsys.Track.NormalizeDofs(-0.2 * phys.Meters(i)), + Cofs: 0} + rsys.Vehicles[i].Reposition(track.Pose{Point: lineupPoint, DAngle: 0}) + rsys.Vehicles[i].SetCmdDriveDspd(0.4, 1.0) + gp.hitPointsL[i] = 2 + gp.hitPointsR[i] = 2 + } +} + +func (gp *BumperCarsGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *BumperCarsGamePhase) VehRankings() []engine.VehRanking { + var score [2]int + for v := 0; v < gp.numVeh; v++ { + score[v] = gp.hitPointsL[v] + gp.hitPointsR[v] + } + rankings := make([]engine.VehRanking, gp.numVeh) + for v := 0; v < gp.numVeh; v++ { + rank := 1 + if score[(v+1)%2] > score[v] { + rank = 2 + } + rankings[v] = engine.VehRanking{ + VehId: v, + Rank: rank, + ScoreString: fmt.Sprintf("%d Hit Points", score[v])} + } + return rankings +} + +func (gp *BumperCarsGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + isDone := false + + // Update hit point indicator lights + for v := 0; v < gp.numVeh; v++ { + if gp.hitPointsL[v] >= 2 { + rsys.Vehicles[v].Lights().Set("h1", colornames.Yellow) + rsys.Vehicles[v].Lights().Set("h2", colornames.Yellow) + } else if gp.hitPointsL[v] == 1 { + rsys.Vehicles[v].Lights().Set("h1", colornames.Black) + rsys.Vehicles[v].Lights().Set("h2", colornames.Yellow) + } else { + rsys.Vehicles[v].Lights().Set("h1", colornames.Black) + rsys.Vehicles[v].Lights().Set("h2", colornames.Black) + } + if gp.hitPointsR[v] >= 2 { + rsys.Vehicles[v].Lights().Set("h4", colornames.Yellow) + rsys.Vehicles[v].Lights().Set("h5", colornames.Yellow) + } else if gp.hitPointsR[v] == 1 { + rsys.Vehicles[v].Lights().Set("h4", colornames.Yellow) + rsys.Vehicles[v].Lights().Set("h5", colornames.Black) + } else { + rsys.Vehicles[v].Lights().Set("h4", colornames.Black) + rsys.Vehicles[v].Lights().Set("h5", colornames.Black) + } + if (gp.hitPointsL[v] + gp.hitPointsR[v]) == 0 { + // you lost! + isDone = true + } + } + + // Process keyboard inputs + for v := 0; v < gp.numVeh; v++ { + veh := &rsys.Vehicles[v] + + dspd := veh.CmdDriveDspd() + if win.JustPressed(buttonMap[v][gameAccel]) { + frames := []light.Frame{light.Frame{Color: colornames.Lime, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "h0", frames, 1) + dspd += 0.1 + if dspd > maxDspd { + dspd = maxDspd + } + veh.SetCmdDriveDspd(dspd, 0.8) + } + if win.JustPressed(buttonMap[v][gameDecel]) { + frames := []light.Frame{light.Frame{Color: colornames.Red, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "h3", frames, 1) + dspd -= 0.1 + if dspd < minDspd { + dspd = minDspd + } + veh.SetCmdDriveDspd(dspd, 0.8) + } + if win.JustPressed(buttonMap[v][gameUturn]) { + veh.CmdUturn(robo.DefUturnRadius) + } + + cofs := veh.CmdDriveCofs() + dCofs := phys.Meters(0) + if win.JustPressed(buttonMap[v][gameCofsL]) { + dCofs = +0.025 + } + if win.JustPressed(buttonMap[v][gameCofsR]) { + dCofs = -0.025 + } + veh.SetCmdDriveCofs(cofs+dCofs, 0.1) + } + + // Collision effects + for _, ce := range rsys.Collider.CurCollisions() { + // display impact points of current collisions, using red dot on the vehicle + for i := 0; i < 2; i++ { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameCirc(ce.VehInfo[i].Id, ce.VehInfo[i].POI, 0.01, colornames.Red, 0)) + } + } + for _, ce := range rsys.Collider.NewCollisions() { + // change cofs/dspd when collisions happen + for i := 0; i < 2; i++ { + cvi := ce.VehInfo[i] + ovi := ce.VehInfo[(i+1)%2] // other vehicle + cv := &rsys.Vehicles[cvi.Id] + ov := &rsys.Vehicles[ovi.Id] + + if cvi.IsRightSideCollision() { + cv.SetCmdDriveCofs(cv.CmdDriveCofs()+0.025, 0.1) + if gp.hitPointsR[cvi.Id] > 0 { + gp.hitPointsR[cvi.Id]-- + } + } + + if cvi.IsLeftSideCollision() { + cv.SetCmdDriveCofs(cv.CmdDriveCofs()-0.025, 0.1) + if gp.hitPointsL[cvi.Id] > 0 { + gp.hitPointsL[cvi.Id]-- + } + } + + if cvi.IsRearCollision() { + dspd := ov.CurDriveDspd() + 0.2 + if dspd > maxDspd { + dspd = maxDspd + } + cv.SetCmdDriveDspd(dspd, 2.0) + } + + if cvi.IsFrontCollision() { + cv.SetCmdDriveDspd(minDspd, 2.0) + } + } + } + + // message board text + rankings := gp.VehRankings() + for v, veh2 := range (*rsys).Vehicles { + vizObj.MBText += fmt.Sprintf("Veh %d %s %s\n", v, veh2.Type(), rankings[v].ScoreString) + } + + return isDone, vizObj +} diff --git a/goverdrive/games/example/sidetap/main.go b/goverdrive/games/example/sidetap/main.go new file mode 100644 index 0000000..9bf8dc4 --- /dev/null +++ b/goverdrive/games/example/sidetap/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("Sidetap (goverdrive)", light.HexPodSpec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &BumperCarsGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/games/example/zoneshapes/game.go b/goverdrive/games/example/zoneshapes/game.go new file mode 100644 index 0000000..c88c20e --- /dev/null +++ b/goverdrive/games/example/zoneshapes/game.go @@ -0,0 +1,168 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + "golang.org/x/image/colornames" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" + "github.com/anki/goverdrive/viz" +) + +var ( + shoulderColor = colornames.Goldenrod + greenColor = colornames.Green + redColor = colornames.Red + purpleColor = colornames.Purple +) + +// ZoneShapesGamePhase defines zones that trigger display of various GameShape +// objects. It is designed to demonstrate: +// - TrackRegion +// - GameShape +type ZoneShapesGamePhase struct { + trShoulder1 *viz.TrackRegion + trShoulder2 *viz.TrackRegion + trGreen *viz.TrackRegion + trRed *viz.TrackRegion + trPurple *viz.TrackRegion +} + +func (gp *ZoneShapesGamePhase) InstructionText(rys *robo.System) string { + return `LEFT/RIGHT ARROW KEYS: Change center offset +UP/DOWN ARROW KEYS: Accelerate and decelerate +RIGHT SHIFT KEY: U-turn +` +} + +func (gp *ZoneShapesGamePhase) Start(rsys *robo.System) { + // Define the (static) track regions at the start of the game + + // The "shoulders" are narrow, full-track-length regions at the sides of the + // track, much like the shoulder of an actual road. + shoulderWidth := phys.Meters(0.02) + gp.trShoulder1 = &viz.TrackRegion{ + Region: *track.NewRegion(&rsys.Track, track.Point{Dofs: 0, Cofs: -rsys.Track.Width() / 2}, rsys.Track.CenLen(), shoulderWidth), + Color: shoulderColor, + } + gp.trShoulder2 = &viz.TrackRegion{ + Region: *track.NewRegion(&rsys.Track, track.Point{Dofs: 0, Cofs: +rsys.Track.Width()/2 - shoulderWidth}, rsys.Track.CenLen(), shoulderWidth), + Color: shoulderColor, + } + + // The colored regions are placed at a few random places, including some + // overlap. Their start point and size should be fine for any normal modular + // track, but could fail hard for unusually small tracks. + gp.trGreen = &viz.TrackRegion{ + Region: *track.NewRegion(&rsys.Track, track.Point{Dofs: 0.3, Cofs: -rsys.Track.Width() / 2}, 1.0, rsys.Track.Width()/2), + Color: greenColor, + } + gp.trRed = &viz.TrackRegion{ + Region: *track.NewRegion(&rsys.Track, track.Point{Dofs: 0.4, Cofs: -rsys.Track.Width() / 2}, 0.5, rsys.Track.Width()), + Color: redColor, + } + dofs := rsys.Track.NormalizeDofs(-1.1) // regions starts from "behind" the finish line + gp.trPurple = &viz.TrackRegion{ + Region: *track.NewRegion(&rsys.Track, track.Point{Dofs: dofs, Cofs: 0.01}, 1.0, rsys.Track.Width()/4), + Color: purpleColor, + } + + // Lineup the vehicle and start driving + numVeh := len(rsys.Vehicles) + if numVeh != 1 { + panic(fmt.Sprintf("ZoneShapesGamePhase is a one-vehicle game; actual numVeh=%d", numVeh)) + } + rsys.Vehicles[0].SetCmdDriveDspd(0.4, 0.4) + rsys.Vehicles[0].Reposition(track.Pose{Point: track.Point{Dofs: 0, Cofs: 0}, DAngle: 0}) +} + +func (gp *ZoneShapesGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *ZoneShapesGamePhase) VehRankings() []engine.VehRanking { + // This example game has no meaningful ranking concept. + // there should only be one vehicle; see Start() + rankings := []engine.VehRanking{ + engine.VehRanking{VehId: 0, Rank: 1, ScoreString: "0"}, + } + return rankings +} + +func (gp *ZoneShapesGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + veh := &rsys.Vehicles[0] // more concise handle to the game's only vehicle + + // Adjust driving speed arrow Up/Down arrow keys are pressed + dspd := veh.CmdDriveDspd() + if win.JustPressed(pixelgl.KeyUp) { + frames := []light.Frame{light.Frame{Color: colornames.Lime, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "guns", frames, 1) + dspd += 0.1 + if dspd > 1.5 { + dspd = 1.5 + } + veh.SetCmdDriveDspd(dspd, 0.4) + } + if win.JustPressed(pixelgl.KeyDown) { + frames := []light.Frame{light.Frame{Color: colornames.Red, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "tail", frames, 1) + dspd -= 0.1 + if dspd < 0.2 { + dspd = 0.2 + } + veh.SetCmdDriveDspd(dspd, 0.4) + } + if win.JustPressed(pixelgl.KeyRightShift) { + veh.CmdUturn(robo.DefUturnRadius) + } + + // Adjust center offset when Left/Right arrow keys are pressed + cofs := veh.CmdDriveCofs() + dCofs := phys.Meters(0) + if win.JustPressed(pixelgl.KeyLeft) { + dCofs = +0.02 + } + if win.JustPressed(pixelgl.KeyRight) { + dCofs = -0.02 + } + veh.SetCmdDriveCofs(cofs+dCofs, 0.1) + + // Draw the track regions + *vizObj.Regions = append(*vizObj.Regions, gp.trShoulder1) + *vizObj.Regions = append(*vizObj.Regions, gp.trShoulder2) + *vizObj.Regions = append(*vizObj.Regions, gp.trGreen) + *vizObj.Regions = append(*vizObj.Regions, gp.trRed) + *vizObj.Regions = append(*vizObj.Regions, gp.trPurple) + + // Track regions trigger game shapes that are anchored to the vehicle + // Reminder: all lengths are in units of phys.Meters + if gp.trShoulder1.ContainsPoint(veh.CurTrackPose().Point) { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewTrackGameLine(0, track.Point{Dofs: -0.05, Cofs: -0.05}, track.Point{Dofs: 0.05, Cofs: -0.05}, shoulderColor, 0.005)) + } + if gp.trShoulder2.ContainsPoint(veh.CurTrackPose().Point) { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewTrackGameLine(0, track.Point{Dofs: -0.05, Cofs: +0.05}, track.Point{Dofs: 0.05, Cofs: +0.05}, shoulderColor, 0.005)) + } + if gp.trGreen.ContainsPoint(veh.CurTrackPose().Point) { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameLine(0, phys.Point{X: 0.05, Y: 0}, phys.Point{X: 0.10, Y: 0}, greenColor, 0.01)) + } + if gp.trRed.ContainsPoint(veh.CurTrackPose().Point) { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameCirc(0, phys.Point{X: -0.1, Y: 0}, 0.03, redColor, 0)) + } + if gp.trPurple.ContainsPoint(veh.CurTrackPose().Point) { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewTrackGameCirc(0, track.Point{Dofs: +0.1, Cofs: 0}, 0.03, purpleColor, 0)) + } + + // Message board text + vizObj.MBText = gp.InstructionText(rsys) + + return false, vizObj +} diff --git a/goverdrive/games/example/zoneshapes/main.go b/goverdrive/games/example/zoneshapes/main.go new file mode 100644 index 0000000..688846d --- /dev/null +++ b/goverdrive/games/example/zoneshapes/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("ZoneShapes (goverdrive)", light.Gen2Spec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &ZoneShapesGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/games/gwenz/chicken/game.go b/goverdrive/games/gwenz/chicken/game.go new file mode 100644 index 0000000..fe36ef5 --- /dev/null +++ b/goverdrive/games/gwenz/chicken/game.go @@ -0,0 +1,287 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + "golang.org/x/image/colornames" + "math" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/gameutil/shapes/persist" + "github.com/anki/goverdrive/gameutil/vehlights" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" + "github.com/anki/goverdrive/viz" +) + +////////////////////////////////////////////////////////////////////// + +type fsmState int // Finite State Machine for the game + +func (s fsmState) String() string { + nameMap := map[fsmState]string{ + stRecover: "stRecover", + stStartPrepare: "stStartPrepare", + stPrepare: "stPrepare", + stCharge: "stCharge", + } + return nameMap[s] +} + +////////////////////////////////////////////////////////////////////// + +const ( + kTie = -1 + winningScore = 5 + + // FSM state names + stRecover fsmState = iota + stStartPrepare fsmState = iota + stPrepare fsmState = iota + stCharge fsmState = iota + + kRecoverTime = 3 * phys.SimSecond + kStartChargeDistMin = -0.20 + kStartChargeDistMax = -0.10 + kRoundDoneDistMin = -0.09 // XXX: must be > kStartChargeDistMax + kRoundDoneDistMax = -0.00 + + kCofsMiss = 0.05 // off-center + kCofsCollide = 0.001 // XXX: tiny bit off-center, to help with collision detection + kCspd = 0.1 + + kDspdRecover = 0.4 + kDaclRecover = 1.0 + kDspdPrepare = 0.8 + kDaclPrepare = 0.2 + kDspdCharge = 1.5 + kDaclCharge = 0.15 +) + +type ChickenGamePhase struct { + numVeh int + state fsmState + tStateBeg phys.SimTime + score [2]int + tSwerve [2]phys.SimTime + didCollide bool + persister *persist.Manager +} + +func (gp *ChickenGamePhase) InstructionText(rys *robo.System) string { + s := `********** CHICKEN ********** +CONTROL + Player 1 Swerve = Left Shift Key + Player 2 Swerve = Right Shift Key + +SCORING + Collision => Player who swerved FIRST gets a point + No Collision => Player who swerved LAST gets a point +` + return s + fmt.Sprintf(" Winner is first to %d points\n", winningScore) +} + +func (gp *ChickenGamePhase) Start(rsys *robo.System) { + gp.numVeh = len(rsys.Vehicles) + if gp.numVeh != 2 { + panic("Chicken requires exactly two vehicles") + } + + // Vehicle "lineup" + dofs0 := rsys.Track.NormalizeDofs(+0.1) + dofs1 := rsys.Track.NormalizeDofs(-0.1) + rsys.Vehicles[0].Reposition(track.Pose{Point: track.Point{Dofs: dofs0, Cofs: +kCofsMiss}, DAngle: 0}) + rsys.Vehicles[1].Reposition(track.Pose{Point: track.Point{Dofs: dofs1, Cofs: -kCofsMiss}, DAngle: math.Pi / 2}) + + gp.state = stRecover + gp.tStateBeg = rsys.Now() + gp.persister = persist.New() +} + +func (gp *ChickenGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *ChickenGamePhase) VehRankings() []engine.VehRanking { + rankings := make([]engine.VehRanking, gp.numVeh) + for v := 0; v < gp.numVeh; v++ { + rank := 1 + if gp.score[v] < gp.score[(v+1)%2] { + rank = 2 + } + rankings[v] = engine.VehRanking{ + VehId: v, + Rank: rank, + ScoreString: fmt.Sprintf("%v", gp.score[v]), + } + } + return rankings +} + +func (gp *ChickenGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + done := false + + // Display the score and status + for v, r := range gp.VehRankings() { + vizObj.MBText += fmt.Sprintf("%s Score %s Speed %.3f\n", + rsys.Vehicles[v].Type(), r.ScoreString, rsys.Vehicles[v].CurDriveDspd()) + } + vizObj.MBText += "\n" + gp.state.String() + "\n" + + // Gather shared FSM inputs + isTrackwise := [2]bool{ + rsys.Vehicles[0].IsFacingTrackwise(), + rsys.Vehicles[1].IsFacingTrackwise()} + now := rsys.Now() + tState := now - gp.tStateBeg // time in current state + + // Compute next state + nextState := gp.state + switch gp.state { + case stStartPrepare: + for v := range rsys.Vehicles { + rsys.Vehicles[v].SetCmdDriveDspd(kDspdPrepare, kDaclPrepare) + rsys.Vehicles[v].SetCmdDriveCofs(kCofsMiss, kCspd) + rsys.Vehicles[v].Lights().Set("top", colornames.Black) + } + nextState = stPrepare + + case stPrepare: + // Prepare = drive slowly off-center, until just after cars pass each other + // (without colliding). This gives a full lap to acclerate for the "chicken + // charge". + deltaDofs := rsys.Track.DriveDeltaDofs(rsys.Vehicles[0].CurTrackPose(), rsys.Vehicles[1].CurTrackPose().Dofs) + vizObj.MBText += fmt.Sprintf("deltaDofs = %v\n", deltaDofs) + justPassed := (deltaDofs >= kStartChargeDistMin) && (deltaDofs <= kStartChargeDistMax) + if /**/ phys.MetersAreNear(rsys.Vehicles[0].CurDriveCofs(), kCofsMiss, 0.01) && + /***/ phys.MetersAreNear(rsys.Vehicles[1].CurDriveCofs(), kCofsMiss, 0.01) && + isTrackwise[0] && !isTrackwise[1] && justPassed { + for v := 0; v < gp.numVeh; v++ { + gp.tSwerve[v] = 0 + rsys.Vehicles[v].SetCmdDriveDspd(kDspdCharge, kDaclCharge) + rsys.Vehicles[v].SetCmdDriveCofs(kCofsCollide, kCspd) + } + gp.didCollide = false + nextState = stCharge + } + + case stCharge: + // top light = speedometer + for v := 0; v < gp.numVeh; v++ { + clr := vehlights.SpeedometerColor(vehlights.DefSpeedometerColors, rsys.Vehicles[v].CurDriveDspd()) + rsys.Vehicles[v].Lights().Set("top", clr) + } + + // Swerve when button is pressed, and remember who swerved first + if (gp.tSwerve[0] == 0) && win.JustPressed(pixelgl.KeyLeftShift) { + gp.tSwerve[0] = now + rsys.Vehicles[0].SetCmdDriveCofs(kCofsMiss, kCspd) + rsys.Vehicles[0].Lights().Set("top", colornames.Black) + } + if (gp.tSwerve[1] == 0) && win.JustPressed(pixelgl.KeyRightShift) { + gp.tSwerve[1] = now + rsys.Vehicles[1].SetCmdDriveCofs(kCofsMiss, kCspd) + rsys.Vehicles[1].Lights().Set("top", colornames.Black) + } + + // Monitor collisions + for _, ce := range rsys.Collider.NewCollisions() { + gp.didCollide = true + // display impact points of current collisions, using red dot on the vehicle + for i := 0; i < 2; i++ { + gp.persister.Add(rsys.Now(), 1000, viz.NewCartesGameCirc(ce.VehInfo[i].Id, ce.VehInfo[i].POI, 0.01, colornames.Red, 0)) + } + } + + deltaDofs := rsys.Track.DriveDeltaDofs(rsys.Vehicles[0].CurTrackPose(), rsys.Vehicles[1].CurTrackPose().Dofs) + roundDone := (deltaDofs <= kRoundDoneDistMax) && (deltaDofs >= kRoundDoneDistMin) + if roundDone { + // determine the winner, update the score, etc + // XXX: Assume simultaneous swerve is very unlikely + + for i := range gp.tSwerve { + if gp.tSwerve[i] == 0 { // never swerved + gp.tSwerve[i] = now + } + } + winner := kTie + if (gp.tSwerve[0] == now) && (gp.tSwerve[1] == now) { + // neither car swerved => tie, even though there was a collision + winner = kTie + } else if gp.didCollide { + // if there was a collision, the person who hesitated is the loser.. + // it's THEIR fault for waiting too long to avoid disaster! + if gp.tSwerve[0] < gp.tSwerve[1] { + winner = 0 + } else { + winner = 1 + } + } else { + // no collision => the scaredy-cat who swerved first is the loser + if gp.tSwerve[0] < gp.tSwerve[1] { + winner = 1 + } else { + winner = 0 + } + } + + // update score, set vehicle lights, etc + for v := 0; v < gp.numVeh; v++ { + clr := colornames.Black + if winner == v { + clr = colornames.Limegreen + gp.score[v]++ + if gp.score[v] >= winningScore { + done = true + } + } else if winner == kTie { + clr = colornames.Yellow + } else { + clr = colornames.Red + } + frames := []light.Frame{ + light.Frame{Color: clr, Tms: 200}, + light.Frame{Color: colornames.Black, Tms: 200}, + } + rsys.Vehicles[v].Lights().SetAnimation(rsys.Now(), "top", frames, light.RepeatForever) + } + nextState = stRecover + } + + case stRecover: + // Make sure vehicles are driving in the opposite direction from one another + if !isTrackwise[0] { + rsys.Vehicles[0].CmdUturn(robo.DefUturnRadius) + } + if isTrackwise[1] { + rsys.Vehicles[1].CmdUturn(robo.DefUturnRadius) + } + + for v := 0; v < gp.numVeh; v++ { + rsys.Vehicles[v].SetCmdDriveDspd(kDspdRecover, kDaclRecover) + rsys.Vehicles[v].SetCmdDriveCofs(kCofsMiss, kCspd) + } + + if (tState > kRecoverTime) && isTrackwise[0] && !isTrackwise[1] { + nextState = stStartPrepare + } + } + + // Assign next state + if nextState != gp.state { + gp.tStateBeg = now + gp.state = nextState + } + + // Update persistent game shapes + *vizObj.Shapes = *gp.persister.Update(rsys.Now()) + + return done, vizObj +} diff --git a/goverdrive/games/gwenz/chicken/main.go b/goverdrive/games/gwenz/chicken/main.go new file mode 100644 index 0000000..3d7eb8c --- /dev/null +++ b/goverdrive/games/gwenz/chicken/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("Chicken (goverdrive)", light.Gen2Spec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &ChickenGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/games/gwenz/connect/game.go b/goverdrive/games/gwenz/connect/game.go new file mode 100644 index 0000000..4f424f4 --- /dev/null +++ b/goverdrive/games/gwenz/connect/game.go @@ -0,0 +1,240 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + cn "golang.org/x/image/colornames" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/gameutil/follow" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/track" + "github.com/anki/goverdrive/viz" +) + +const ( + vLeader = 0 + vFollow = 1 + vPlayer = 2 + + // form = formation + formDspd = 0.80 // nominal speed + formDspdDelta = 0.10 // adjustment to maintain formation + formDacl = 2.0 + formCspd = 0.05 + formDofsDelta = 0.10 + formDofsBuf = 0.04 + formCofsDelta = 0.02 + formMaxDofs = 0.40 + + playerSlowDspd = 0.4 + playerFastDspd = 1.2 + playerDacl = 0.5 + playerCspd = 0.2 + + newAttemptDist = 0.4 + goalRadius = 0.04 +) + +type fsmState int + +const ( + stAttempt = iota + stGoalHit = iota + stSuccess = iota + stFail = iota +) + +// ConnectGamePhase does simple driving for a set of vehicles. +type ConnectGamePhase struct { + numVeh int + score int + playerDesDspd phys.MetersPerSec + playerDesCofs phys.Meters + state fsmState + follower *follow.Follower +} + +func (gp *ConnectGamePhase) InstructionText(rys *robo.System) string { + return `LEADER CAR: Q,E Keys +FOLLOW CAR: S,W Keys +PLAYER CAR: Arrow keys, Right Shift Key +` +} + +func (gp *ConnectGamePhase) Start(rsys *robo.System) { + gp.numVeh = len(rsys.Vehicles) + if gp.numVeh != 3 { + panic("Game requires exactly 3 vehicles!") + } + + // initial formation + rsys.Vehicles[vLeader].Reposition(track.Pose{Point: track.Point{Dofs: 0.1, Cofs: 0}, DAngle: 0}) + rsys.Vehicles[vFollow].Reposition(track.Pose{Point: track.Point{Dofs: 0.0, Cofs: 0}, DAngle: 0}) + rsys.Vehicles[vLeader].SetCmdDriveDspd(formDspd, formDacl) + rsys.Vehicles[vFollow].SetCmdDriveDspd(formDspd, formDacl) + + gp.playerDesDspd = playerSlowDspd + gp.playerDesCofs = rsys.Track.Width() / 2 + rsys.Vehicles[vPlayer].Reposition(track.Pose{Point: track.Point{Dofs: 0.0, Cofs: gp.playerDesCofs}, DAngle: 0}) + rsys.Vehicles[vPlayer].SetCmdDriveDspd(gp.playerDesDspd, playerDacl) + + gp.score = 0 + gp.state = stAttempt + gp.follower = follow.New(vLeader, vFollow, -0.4, 0.0, formDacl, formCspd, rsys.Track.CenLen(), rsys.Now(), 0) +} + +func (gp *ConnectGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *ConnectGamePhase) VehRankings() []engine.VehRanking { + rankings := make([]engine.VehRanking, gp.numVeh) + for v := 0; v < gp.numVeh; v++ { + rankings = append(rankings, engine.VehRanking{VehId: v, Rank: v, ScoreString: "0"}) + } + return rankings +} + +func (gp *ConnectGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + // concise pointers to (not copies of!!) game vehicles + lVeh := &rsys.Vehicles[vLeader] + fVeh := &rsys.Vehicles[vFollow] + pVeh := &rsys.Vehicles[vPlayer] + + ltpose := lVeh.CurTrackPose() + ftpose := fVeh.CurTrackPose() + ptpose := pVeh.CurTrackPose() + + // Adjust position of the leader car + cofs := lVeh.CmdDriveCofs() + dCofs := phys.Meters(0) + if win.JustPressed(pixelgl.KeyQ) { + dCofs = +0.025 + } + if win.JustPressed(pixelgl.KeyE) { + dCofs = -0.025 + } + lVeh.SetCmdDriveCofs(cofs+dCofs, 0.1) + + // Adjust desired position of the Follow car + followDofs := gp.follower.TargetDeltaDofs() + if win.JustPressed(pixelgl.KeyW) { + followDofs += formDofsDelta + } + if win.JustPressed(pixelgl.KeyS) { + followDofs -= formDofsDelta + } + if followDofs <= (-rsys.Track.CenLen() / 2) { + followDofs = (-rsys.Track.CenLen() / 2) + 0.001 + } + if followDofs > formMaxDofs { + followDofs = formMaxDofs + } + vizObj.MBText += fmt.Sprintf("followDofs %.3f\n", followDofs) + gp.follower.SetTargetDeltaDofs(followDofs) + gp.follower.Update(rsys) + + // Player controls + // can't issue new player command until previous command is finished + lcolor := cn.Black + if phys.MetersPerSecAreNear(pVeh.CurDriveDspd(), gp.playerDesDspd, 0.02) && + phys.MetersAreNear(pVeh.CurDriveCofs(), gp.playerDesCofs, 0.002) { + // new player command ok + if win.JustPressed(pixelgl.KeyRightShift) { + pVeh.CmdUturn(robo.DefUturnRadius) + } + + // speed + if win.JustPressed(pixelgl.KeyUp) { + gp.playerDesDspd = playerFastDspd + } + if win.JustPressed(pixelgl.KeyDown) { + gp.playerDesDspd = playerSlowDspd + } + pVeh.SetCmdDriveDspd(gp.playerDesDspd, playerDacl) + + // center offset + if win.JustPressed(pixelgl.KeyLeft) { + gp.playerDesCofs = rsys.Track.Width() / 2 + } + if win.JustPressed(pixelgl.KeyRight) { + gp.playerDesCofs = -(rsys.Track.Width() / 2) + } + pVeh.SetCmdDriveCofs(gp.playerDesCofs, playerCspd) + } else { + lcolor = cn.Violet + } + + // Game state and scoring + isFar := (rsys.Track.DofsDist(ltpose.Dofs, ptpose.Dofs) > newAttemptDist) && (rsys.Track.DofsDist(ftpose.Dofs, ptpose.Dofs) > newAttemptDist) + didCollide := false + for _, ce := range rsys.Collider.NewCollisions() { + if (ce.VehInfo[0].Id == vPlayer) || (ce.VehInfo[1].Id == vPlayer) { + didCollide = true + } + } + + switch gp.state { + case stAttempt: + deltaDofs := rsys.Track.DriveDeltaDofs(ltpose, ftpose.Dofs) + tposeGoal := track.Pose{ + Point: track.Point{ + Dofs: ltpose.Dofs + (deltaDofs / 2), + Cofs: (ftpose.Cofs + ltpose.Cofs) / 2}, + DAngle: 0, + } + pGoal := rsys.Track.ToPose(tposeGoal).Point + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameCirc(-1, pGoal, goalRadius, cn.White, 0.004)) + + pPlayer := rsys.Track.ToPose(pVeh.CurTrackPose()).Point + if phys.Dist(pGoal, pPlayer) < goalRadius { + gp.state = stGoalHit + } else if didCollide { + gp.state = stFail + } + case stGoalHit: + // Player hit the goal, but doesn't score a point until we are sure player + // didn't later collide with one of the formation cars. + lcolor = cn.Yellow + plDeltaDofs := rsys.Track.DriveDeltaDofs(ptpose, ltpose.Dofs) + pfDeltaDofs := rsys.Track.DriveDeltaDofs(ptpose, ftpose.Dofs) + bothAhead := (plDeltaDofs > 0) && (pfDeltaDofs > 0) + bothBehind := (plDeltaDofs < 0) && (pfDeltaDofs < 0) + if didCollide { + gp.state = stFail + } else if bothAhead || bothBehind { + gp.score++ + gp.state = stSuccess + } + case stSuccess: + lcolor = cn.Limegreen + if isFar { + gp.state = stAttempt + } + case stFail: + lcolor = cn.Red + if isFar { + gp.state = stAttempt + } + } + + // Display game state (vehicle lights, message board, etc) + for _, ce := range rsys.Collider.CurCollisions() { + // impact points of current collisions, using red dot on the vehicle + for i := 0; i < 2; i++ { + *vizObj.Shapes = append(*vizObj.Shapes, viz.NewCartesGameCirc(ce.VehInfo[i].Id, ce.VehInfo[i].POI, 0.01, cn.Red, 0)) + } + } + pVeh.Lights().Set("top", lcolor) + vizObj.MBText += gp.InstructionText(rsys) + "\n" + vizObj.MBText += fmt.Sprintf("Player Score: %d\n", gp.score) + + return false, vizObj +} diff --git a/goverdrive/games/gwenz/connect/main.go b/goverdrive/games/gwenz/connect/main.go new file mode 100644 index 0000000..2977cd9 --- /dev/null +++ b/goverdrive/games/gwenz/connect/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("Connect3 (goverdrive)", light.Gen2Spec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &ConnectGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/games/gwenz/fourmation/game.go b/goverdrive/games/gwenz/fourmation/game.go new file mode 100644 index 0000000..cb17a45 --- /dev/null +++ b/goverdrive/games/gwenz/fourmation/game.go @@ -0,0 +1,170 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "fmt" + "golang.org/x/image/colornames" + + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/gameutil/follow" + "github.com/anki/goverdrive/gameutil/vehlights" + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" + _ "github.com/anki/goverdrive/viz" +) + +const ( + minDspd = 0.3 + maxDspd = 1.5 + followDacl = 3.0 + followCspd = 0.1 + numVeh = 4 + numFormations = 9 +) + +// FourmationGamePhase drives a set of four vehicles in a formation. +type FourmationGamePhase struct { + followers []*follow.Follower + curFormation int +} + +type formation struct { + dDofs1 phys.Meters + dCofs1 phys.Meters + dDofs2 phys.Meters + dCofs2 phys.Meters + dDofs3 phys.Meters + dCofs3 phys.Meters +} + +func (gp *FourmationGamePhase) changeFormation(f int) { + if f > numFormations { + panic(fmt.Sprintf("Formation number %d is invalid", f)) + } + formationList := [numFormations]formation{ + formation{dDofs1: -0.10, dCofs1: +0.10, dDofs2: -0.10, dCofs2: -0.10, dDofs3: -0.20, dCofs3: +0.00}, // diamond + formation{dDofs1: -0.20, dCofs1: +0.10, dDofs2: -0.20, dCofs2: -0.10, dDofs3: -0.10, dCofs3: +0.00}, // Y + formation{dDofs1: -0.60, dCofs1: +0.00, dDofs2: -0.40, dCofs2: +0.00, dDofs3: -0.20, dCofs3: +0.00}, // vert line + formation{dDofs1: -0.00, dCofs1: +0.10, dDofs2: -0.00, dCofs2: +0.20, dDofs3: -0.00, dCofs3: -0.10}, // horz line + formation{dDofs1: -0.00, dCofs1: +0.10, dDofs2: -0.13, dCofs2: +0.10, dDofs3: -0.13, dCofs3: +0.00}, // square + formation{dDofs1: -0.00, dCofs1: +0.10, dDofs2: -0.13, dCofs2: +0.10, dDofs3: +0.00, dCofs3: -0.10}, // L + formation{dDofs1: -0.13, dCofs1: +0.00, dDofs2: -0.13, dCofs2: -0.10, dDofs3: +0.13, dCofs3: -0.00}, // L rotated + formation{dDofs1: -0.00, dCofs1: +0.10, dDofs2: -0.13, dCofs2: +0.00, dDofs3: -0.13, dCofs3: -0.10}, // Z + formation{dDofs1: -0.13, dCofs1: +0.10, dDofs2: -0.13, dCofs2: +0.00, dDofs3: -0.00, dCofs3: -0.10}, // Z rotated + } + fmtn := formationList[f] + gp.followers[0].SetTargetDeltaDofs(fmtn.dDofs1) + gp.followers[0].SetTargetDeltaCofs(fmtn.dCofs1) + gp.followers[1].SetTargetDeltaDofs(fmtn.dDofs2) + gp.followers[1].SetTargetDeltaCofs(fmtn.dCofs2) + gp.followers[2].SetTargetDeltaDofs(fmtn.dDofs3) + gp.followers[2].SetTargetDeltaCofs(fmtn.dCofs3) +} + +func (gp *FourmationGamePhase) InstructionText(rys *robo.System) string { + return `SPACE BAR: Next formation +LEFT/RIGHT ARROW KEYS: Change horizontal offset +UP/DOWN ARROW KEYS: Accelerate and decelerate +RIGHT SHIFT KEY: U-turn +` +} + +func (gp *FourmationGamePhase) Start(rsys *robo.System) { + if len(rsys.Vehicles) != numVeh { + panic(fmt.Sprintf("Exactly %d vehicles are required", numVeh)) + } + + for i, _ := range rsys.Vehicles { + lineupPoint := track.Point{ + Dofs: rsys.Track.NormalizeDofs(-0.2 * phys.Meters(i)), + Cofs: 0} + rsys.Vehicles[i].Reposition(track.Pose{Point: lineupPoint, DAngle: 0}) + rsys.Vehicles[i].SetCmdDriveDspd(0.4, 1.0) + } + + gp.followers = make([]*follow.Follower, numVeh-1) + for v := 0; v < (numVeh - 1); v++ { + vFollow := v + 1 + deltaDofs := phys.Meters(-0.2 - (float32(v) * 0.2)) + deltaCofs := phys.Meters(0) + gp.followers[v] = follow.New(0, vFollow, deltaDofs, deltaCofs, followDacl, followCspd, rsys.Track.CenLen(), rsys.Now(), 0) + } + gp.curFormation = 0 + gp.changeFormation(gp.curFormation) +} + +func (gp *FourmationGamePhase) Stop(rsys *robo.System) { + // no-op +} + +func (gp *FourmationGamePhase) VehRankings() []engine.VehRanking { + rankings := make([]engine.VehRanking, numVeh) + for v := 0; v < numVeh; v++ { + rankings[v] = engine.VehRanking{VehId: v, Rank: v, ScoreString: "0"} + } + return rankings +} + +func (gp *FourmationGamePhase) Update(rsys *robo.System, win *pixelgl.Window) (bool, engine.GamePhaseVizObjects) { + vizObj := engine.EmptyGamePhaseVizObjects() + veh := &rsys.Vehicles[0] + + if win.JustPressed(pixelgl.KeySpace) { + gp.curFormation = (gp.curFormation + 1) % numFormations + gp.changeFormation(gp.curFormation) + } + + dspd := veh.CmdDriveDspd() + if win.JustPressed(pixelgl.KeyUp) { + frames := []light.Frame{light.Frame{Color: colornames.Lime, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "guns", frames, 1) + dspd += 0.1 + if dspd > maxDspd { + dspd = maxDspd + } + veh.SetCmdDriveDspd(dspd, 0.4) + } + if win.JustPressed(pixelgl.KeyDown) { + frames := []light.Frame{light.Frame{Color: colornames.Red, Tms: 200}} + veh.Lights().SetAnimation(rsys.Now(), "tail", frames, 1) + dspd -= 0.1 + if dspd < minDspd { + dspd = minDspd + } + veh.SetCmdDriveDspd(dspd, 0.4) + } + if win.JustPressed(pixelgl.KeyRightShift) { + veh.CmdUturn(robo.DefUturnRadius) + } + + cofs := veh.CmdDriveCofs() + dCofs := phys.Meters(0) + if win.JustPressed(pixelgl.KeyLeft) { + dCofs = +0.025 + } + if win.JustPressed(pixelgl.KeyRight) { + dCofs = -0.025 + } + veh.SetCmdDriveCofs(cofs+dCofs, 0.1) + + // followers + for v := 1; v < numVeh; v++ { + gp.followers[v-1].Update(rsys) + } + + // speedometer light + clr := vehlights.SpeedometerColor(vehlights.DefSpeedometerColors, veh.CurDriveDspd()) + veh.Lights().Set("top", clr) + + // message board text + vizObj.MBText += fmt.Sprintf("Formation %d\n", gp.curFormation) + vizObj.MBText += gp.InstructionText(rsys) + "\n" + + return false, vizObj +} diff --git a/goverdrive/games/gwenz/fourmation/main.go b/goverdrive/games/gwenz/fourmation/main.go new file mode 100644 index 0000000..4b551af --- /dev/null +++ b/goverdrive/games/gwenz/fourmation/main.go @@ -0,0 +1,38 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package main + +import ( + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/engine" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/viz" +) + +func run() { + // Configure standard parts of the game from command-line args + gameConfig := engine.NewCLIGameConfig("Fourmation (goverdrive)", light.Gen2Spec) + + // Create the remaining game components + primViz := viz.NewPixelViz() + worldViz := viz.NewPixelWorldViz(primViz, gameConfig.Track()) + rsim := robo.NewIdealSimulator() + rcollide := robo.NewCollisionDetector(gameConfig.Track(), gameConfig.Vehicles()) + roboSys := robo.NewSystem(gameConfig.Track(), gameConfig.Vehicles(), rsim, rcollide) + + // Run the game + vizCfg := engine.GamePhaseVizConfig{ + ShowInstr: gameConfig.ShowInstructions(), + MsgBoardPixHeight: gameConfig.MsgBoardPixHeight(), + WorldViz: worldViz, + Window: gameConfig.Window(), + } + engine.RunGameLoop(vizCfg, roboSys, &FourmationGamePhase{}) +} + +func main() { + pixelgl.Run(run) +} diff --git a/goverdrive/gameutil/follow/follow.go b/goverdrive/gameutil/follow/follow.go new file mode 100644 index 0000000..327a5da --- /dev/null +++ b/goverdrive/gameutil/follow/follow.go @@ -0,0 +1,133 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package follow + +import ( + "fmt" + "math" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" +) + +// Follower issues commands to a "follower" vehicle, to maintain a positional +// relationship relative to a "leader" vehicle. Two or more Followers can be +// used to create a multi-vehicle formation. +type Follower struct { + vLeader int + vFollow int + targetDeltaDofs phys.Meters + targetDeltaCofs phys.Meters + dacl phys.MetersPerSec2 + cspd phys.MetersPerSec + trackLen phys.Meters + adjustPeriod phys.SimTime // how often to adjust speed/position of follow vehicle + nextUpdateTime phys.SimTime +} + +const ( + maxDofsDistNear = 0.010 + maxCofsDistNear = 0.002 + majorCatchupFactor = 1.25 + majorFallbackFactor = 0.75 + minorCatchupFactor = 1.05 + minorFallbackFactor = 0.95 +) + +// New returns a pointer to a new Follow object. +func New(vLeader, vFollow int, + targetDeltaDofs, targetDeltaCofs phys.Meters, + dacl phys.MetersPerSec2, + cspd phys.MetersPerSec, + trackLen phys.Meters, + now, adjustPeriod phys.SimTime) *Follower { + + c := Follower{ + vLeader: vLeader, + vFollow: vFollow, + dacl: dacl, + cspd: cspd, + trackLen: trackLen, + adjustPeriod: adjustPeriod, + nextUpdateTime: now, + } + c.SetTargetDeltaDofs(targetDeltaDofs) + c.SetTargetDeltaCofs(targetDeltaCofs) + + return &c +} + +func (c *Follower) TargetDeltaDofs() phys.Meters { + return c.targetDeltaDofs +} + +func (c *Follower) TargetDeltaCofs() phys.Meters { + return c.targetDeltaCofs +} + +func (c *Follower) SetTargetDeltaDofs(target phys.Meters) { + if math.Abs(float64(target)) >= float64(c.trackLen/2) { + panic(fmt.Sprintf("target=%v is too large; must be less than (trackLen/2)=%v", target, c.trackLen/2)) + } + c.targetDeltaDofs = target +} + +func (c *Follower) SetTargetDeltaCofs(target phys.Meters) { + if math.Abs(float64(target)) >= float64(c.trackLen/2) { + panic(fmt.Sprintf("target=%v is too large; must be less than (trackLen/2)=%v", target, c.trackLen/2)) + } + c.targetDeltaCofs = target +} + +// Update issues new vehicle commands, if needed, to maintain the desired +// leader-follower positional relationship. true is returned if the follower is +// sufficiently near its relative target position. +func (c *Follower) Update(rsys *robo.System) bool { + // l=leader, f=follower (for brevity) + lVeh := &rsys.Vehicles[c.vLeader] + fVeh := &rsys.Vehicles[c.vFollow] + + deltaDist := rsys.Track.DriveDeltaDist(lVeh.CurTrackPose(), fVeh.CurTrackPose().Dofs) + deltaDofsErrAmt := deltaDist - c.targetDeltaDofs + deltaCofsErrAmt := rsys.Track.DriveDeltaCofs(lVeh.CurTrackPose(), fVeh.CmdTrackCofs()) - c.targetDeltaCofs + + if rsys.Now() >= c.nextUpdateTime { + c.nextUpdateTime = rsys.Now() + c.adjustPeriod + + // Driving direction + if fVeh.IsFacingTrackwise() != lVeh.IsFacingTrackwise() { + fVeh.CmdUturn(robo.DefUturnRadius) + return false + } + + // Dofs Speed + lRp := rsys.Track.Rp(rsys.Track.RpiAt(lVeh.CurTrackPose().Dofs)) + fDspd := lVeh.CurDriveDspd() + if !lRp.IsStraight() { + fDspd *= phys.MetersPerSec(lRp.CurveRadius(fVeh.CurTrackPose().Cofs) / lRp.CurveRadius(lVeh.CurTrackPose().Cofs)) + } + if deltaDofsErrAmt > (+maxDofsDistNear) { + // ahead of desired position => fall back + fDspd *= majorFallbackFactor + } else if deltaDofsErrAmt < (-maxDofsDistNear) { + // behind desired position => catch up + fDspd *= majorCatchupFactor + } else if deltaDofsErrAmt > 0 { + fDspd *= minorFallbackFactor + } else if deltaDofsErrAmt < 0 { + fDspd *= minorCatchupFactor + } + fVeh.SetCmdDriveDspd(fDspd, c.dacl) + //fmt.Printf("deltaDist=%v deltaDofsErrAmt=%v fDspd=%v\n", deltaDist, deltaDofsErrAmt, fDspd) + + // Cofs + if math.Abs(float64(deltaCofsErrAmt)) > maxCofsDistNear { + fVeh.SetCmdDriveCofs(lVeh.CurDriveCofs()+c.targetDeltaCofs, c.cspd) + } + } + + // return value = "follower is near target position" + return (math.Abs(float64(deltaDofsErrAmt)) <= maxDofsDistNear) && + /**/ (math.Abs(float64(deltaCofsErrAmt)) <= maxCofsDistNear) +} diff --git a/goverdrive/gameutil/lapmetrics/lapmetrics.go b/goverdrive/gameutil/lapmetrics/lapmetrics.go new file mode 100644 index 0000000..85d818e --- /dev/null +++ b/goverdrive/gameutil/lapmetrics/lapmetrics.go @@ -0,0 +1,132 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package lapmetrics provides track lap count, duration, path length, and other +// lap metrics for all vehicles involved in the game. +package lapmetrics + +import ( + "fmt" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/track" +) + +// CompletedLapInfo stores information about a completed lap. +type CompletedLapInfo struct { + LapNumber int + LapTime phys.SimTime + IsTrackwise bool // false => counter-trackwise + PathLen phys.Meters // actual driving path length + MinDspd phys.MetersPerSec + MaxDspd phys.MetersPerSec +} + +// VehLapInfo stores completed laps and current lap info for one vehicle. +type VehLapInfo struct { + curLapStartOdom phys.Meters + curLapStartTime phys.SimTime + curLapMinDspd phys.MetersPerSec + curLapMaxDspd phys.MetersPerSec + doneLaps []CompletedLapInfo + numNewReportedLaps int +} + +func (cli *CompletedLapInfo) String() string { + durSeconds := float64(cli.LapTime) / float64(phys.SimSecond) + return fmt.Sprintf("LapNumber=%v, IsTrackwise=%v, LapTime=%.3f sec, PathLen=%.3f, MinDspd=%.3f, MaxDspd=%.3f", + cli.LapNumber, cli.IsTrackwise, durSeconds, cli.PathLen, cli.MinDspd, cli.MaxDspd) +} + +////////////////////////////////////////////////////////////////////// + +// LapMetrics stores track VehLapInfo for all vehicles +type LapMetrics struct { + recordTrackwiseLaps bool + recordCounterTrackwiseLaps bool + info []VehLapInfo +} + +// New returns a fresh LapMetrics object, which starts measuring from the +// current speed, odom, etc of the vehicles. +func New(now phys.SimTime, vehs *[]robo.Vehicle, recordTrackwiseLaps, recordCounterTrackwiseLaps bool) *LapMetrics { + lm := LapMetrics{ + recordTrackwiseLaps: recordTrackwiseLaps, + recordCounterTrackwiseLaps: recordCounterTrackwiseLaps, + info: make([]VehLapInfo, len(*vehs)), + } + for v, veh := range *vehs { + lm.info[v] = VehLapInfo{ + curLapStartOdom: veh.Odom(), + curLapStartTime: now, + curLapMinDspd: veh.CurDriveDspd(), + curLapMaxDspd: veh.CurDriveDspd(), + doneLaps: make([]CompletedLapInfo, 0), + } + } + return &lm +} + +// NumLapsCompleted returns the number laps that a vehicle has completed. +func (lm *LapMetrics) NumLapsCompleted(v int) int { + return len(lm.info[v].doneLaps) +} + +// AllCompletedLapInfo returns all completed lap info for a particular vehicle. +func (lm *LapMetrics) AllCompletedLapInfo(v int) []CompletedLapInfo { + return lm.info[v].doneLaps +} + +// NewCompletedLapInfo returns info about all newly completed laps, ie since the +// last call to NewCompletedLapInfo. +func (lm *LapMetrics) NewCompletedLapInfo(v int) []CompletedLapInfo { + newLapInfo := make([]CompletedLapInfo, 0) // empty + numCompl := lm.NumLapsCompleted(v) + if lm.info[v].numNewReportedLaps < numCompl { + newLapInfo = lm.info[v].doneLaps[lm.info[v].numNewReportedLaps:numCompl] + lm.info[v].numNewReportedLaps = numCompl + } + return newLapInfo +} + +// Update is the "tick" that should be called from the game phase's Update(). +func (lm *LapMetrics) Update(now phys.SimTime, trk *track.Track, vehs *[]robo.Vehicle) { + for v, veh := range *vehs { + // Update current lap's min/max values + curDspd := veh.CurDriveDspd() + if curDspd < lm.info[v].curLapMinDspd { + lm.info[v].curLapMinDspd = curDspd + } + if curDspd > lm.info[v].curLapMaxDspd { + lm.info[v].curLapMaxDspd = curDspd + } + + lapDist := veh.Odom() - lm.info[v].curLapStartOdom + // TODO(gwenz): Review and tune lap thresholds + if veh.CurDriveDofs() < 0.10 { + // restart lap tracking + if lapDist >= (0.7 * trk.CenLen()) { + // succesfully completed a lap + lapDist -= veh.CurDriveDofs() + isTrackwise := veh.IsFacingTrackwise() + if (isTrackwise && lm.recordTrackwiseLaps) || + (!isTrackwise && lm.recordCounterTrackwiseLaps) { + newLap := CompletedLapInfo{ + LapNumber: len(lm.info[v].doneLaps) + 1, + LapTime: now - lm.info[v].curLapStartTime, + IsTrackwise: isTrackwise, + PathLen: lapDist, + MinDspd: lm.info[v].curLapMinDspd, + MaxDspd: lm.info[v].curLapMaxDspd, + } + lm.info[v].doneLaps = append(lm.info[v].doneLaps, newLap) + } + } + lm.info[v].curLapStartOdom = veh.Odom() - veh.CurDriveDofs() + lm.info[v].curLapStartTime = now + lm.info[v].curLapMinDspd = curDspd + lm.info[v].curLapMaxDspd = curDspd + } + } +} diff --git a/goverdrive/gameutil/shapes/persist/persist.go b/goverdrive/gameutil/shapes/persist/persist.go new file mode 100644 index 0000000..811e9e1 --- /dev/null +++ b/goverdrive/gameutil/shapes/persist/persist.go @@ -0,0 +1,79 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package persist helps manage game shapes that persist for >1 tick. +package persist + +import ( + _ "fmt" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/viz" +) + +// shapeNode is a linked-list node for a persistent shape. A linked list is used +// because it has O(1) add/remove time. +type shapeNode struct { + shape *viz.GameShape + tExpire phys.SimTime + next *shapeNode +} + +// Manager manages persistent shapes +type Manager struct { + head *shapeNode + tail *shapeNode + numElem uint +} + +// New returns a manager with no shapes +func New() *Manager { + return &Manager{head: nil, tail: nil, numElem: 0} +} + +// Add adds a GameShape to the persistence manager. The duration to persist is +// in milliseconds (for convenience). +func (m *Manager) Add(now phys.SimTime, msDur uint, shape *viz.GameShape) { + node := shapeNode{ + shape: shape, + tExpire: now + (phys.SimTime(msDur) * phys.SimMillisecond), + next: nil, + } + + // add to end of linked list + if m.tail == nil { // empty + m.head = &node + m.tail = &node + } else { + m.tail.next = &node + m.tail = &node + } + m.numElem++ +} + +// Update removes all GameShape objects that have expired, and returns a list of +// shapes that have not yet expired. +func (m *Manager) Update(now phys.SimTime) *[]*viz.GameShape { + vizShapes := make([]*viz.GameShape, 0, m.numElem) + var prev *shapeNode = nil + for cur := m.head; cur != nil; { + if cur.tExpire > now { + vizShapes = append(vizShapes, cur.shape) + prev = cur + } else { + // shape expired => do not draw, and remove it from linked list + // no change to prev + m.numElem-- + if cur.next == nil { + m.tail = prev + } + if prev == nil { + m.head = cur.next + } else { + prev.next = cur.next + } + } + cur = cur.next + } + return &vizShapes +} diff --git a/goverdrive/gameutil/vehlights/speedometer.go b/goverdrive/gameutil/vehlights/speedometer.go new file mode 100644 index 0000000..16a1c1c --- /dev/null +++ b/goverdrive/gameutil/vehlights/speedometer.go @@ -0,0 +1,69 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package vehlights provides additional functionality for vehicle light +// effects. +package vehlights + +import ( + _ "fmt" + "golang.org/x/image/colornames" + "image/color" + + "github.com/anki/goverdrive/phys" +) + +type SpeedColorPair struct { + Speed phys.MetersPerSec + Color color.Color +} + +// DefSpeedometerColors is a nice "default" color map for using a vehicle light +// as a speedometer. +var DefSpeedometerColors = []SpeedColorPair{ + SpeedColorPair{0.3, colornames.Black}, + SpeedColorPair{0.7, colornames.Darkkhaki}, + SpeedColorPair{1.0, colornames.Lime}, + SpeedColorPair{1.4, colornames.White}, +} + +// SpeedometerColor chooses a color by linearly interpolating between N many +// user-defined points in color space. +func SpeedometerColor(clrMap []SpeedColorPair, speed phys.MetersPerSec) color.Color { + n := len(clrMap) + if n == 0 { + return color.Black + } + + // edge-case behavior + if speed < clrMap[0].Speed { + return clrMap[0].Color + } + if speed >= clrMap[n-1].Speed { + return clrMap[n-1].Color + } + + // interpolate + for i := 0; i < (n - 1); i++ { + if clrMap[i+1].Speed > speed { + percent := (float64(speed) - float64(clrMap[i].Speed)) / (float64(clrMap[i+1].Speed) - float64(clrMap[i].Speed)) + var a uint32 + c1 := make([]uint32, 3) + c2 := make([]uint32, 3) + c1[0], c1[1], c1[2], a = clrMap[i+0].Color.RGBA() + c2[0], c2[1], c2[2], _ = clrMap[i+1].Color.RGBA() + c := make([]uint8, 3) + for i := 0; i < 3; i++ { + if c2[i] > c1[i] { + c[i] = uint8(c1[i]) + uint8(percent*float64(uint8(c2[i])-uint8(c1[i]))) + } else { + c[i] = uint8(c1[i]) - uint8(percent*float64(uint8(c1[i])-uint8(c2[i]))) + } + } + //fmt.Printf("i=%v percent=%v (%v %v %v) (%v %v %v) => %v %v %v\n", i, percent, c1[0], c1[1], c1[2], c2[0], c2[1], c2[2], c[0], c[1], c[2]) + return color.RGBA{R: c[0], G: c[1], B: c[2], A: uint8(a)} + } + } + panic("CalcSpeedometerColor reached end of function") + return color.RGBA{0, 0, 0, 0} +} diff --git a/goverdrive/phys/coord.go b/goverdrive/phys/coord.go new file mode 100644 index 0000000..911341f --- /dev/null +++ b/goverdrive/phys/coord.go @@ -0,0 +1,113 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package phys + +import ( + "fmt" + "math" +) + +// Point represents a point in traditional Cartesian space. +// X is the primary axis, Y is the secondary axis. +// X>0 => right +// X<0 => left +// Y>0 => up +// Y<0 => down +type Point struct { + X Meters + Y Meters +} + +func (p Point) String() string { + return fmt.Sprintf("Point{X: %v, Y: %v}", p.X, p.Y) +} + +// Pose represents a location and an orientation. +// Theta==0 => facing primary axis (eg +X, or right) +// Theta==pi/2 => facing secondary axis (eg +Y, or up) +// Theta==pi => facing away from primary axis (eg -X, or left) +// Theta==3*pi/2 => facing away from secondary axis (eg -Y, or down) +// Theta==-pi/2 => same as (3*pi/2) +type Pose struct { + Point + Theta Radians +} + +func (p Pose) String() string { + return fmt.Sprintf("Pose{X: %v, Y: %v, Theta: %v}", p.X, p.Y, p.Theta) +} + +// Dist returns the Cartesian distance between two points. +func Dist(p1, p2 Point) Meters { + dx := (p1.X - p2.X) + dy := (p1.Y - p2.Y) + dist := math.Sqrt(float64((dx * dx) + (dy * dy))) + return Meters(dist) +} + +// AdvancePose advances by p2, starting from Pose p1. +// Specifically: +// 1. X/Y translation in the direction of p1.Theta +// 2. Rotation by p2.Theta +func (p1 Pose) AdvancePose(p2 Pose) Pose { + // Assume p1 is at the origin, and move by p2's X,Y in p1.Theta direction + pp := Point{X: p2.X, Y: p2.Y}.ToPolarPoint() + pp.A = NormalizeRadians(p1.Theta + pp.A) + p := pp.ToPoint() + + // Add original displacemet of p1 + // DEBUG: fmt.Printf("pp=%s\n p=%s\n p1=%s\n\n", pp.String(), p.String(), p1.String()) + p.X += p1.X + p.Y += p1.Y + + // Compute orientation of final pose + pose := Pose{Point: Point{X: p.X, Y: p.Y}, Theta: p1.Theta + p2.Theta} + pose.Theta = NormalizeRadians(pose.Theta) + return pose +} + +// RelativeTo expresses pose p1 relative to pose p2 frame-of-reference. +func (p1 Pose) RelativeTo(p2 Pose) Pose { + // translate point to origin + xlatePoint := Point{X: p1.X - p2.X, Y: p1.Y - p2.Y} + pp := xlatePoint.ToPolarPoint() + + // rotate about the origin + pp.A = NormalizeRadians(pp.A - p2.Theta) + p := pp.ToPoint() + + // correct the new pose angle + pose := Pose{Point: p, Theta: p1.Theta - p2.Theta} + pose.Theta = NormalizeRadians(pose.Theta) + + return pose +} + +// PolarPoint is a polar representation of a point, ie radius + angle. +type PolarPoint struct { + R Meters + A Radians +} + +func (pp PolarPoint) String() string { + return fmt.Sprintf("PolarPoint{R: %v, A: %v}", pp.R, pp.A) +} + +// ToPolarPoint converts a Cartesian Point to its PolarPoint representation. +func (p Point) ToPolarPoint() PolarPoint { + r := Meters(math.Sqrt(float64((p.X * p.X) + (p.Y * p.Y)))) + a := Radians(math.Atan2(float64(p.Y), float64(p.X))) + a = NormalizeRadians(a) // TODO: Is this the right thing to do? + return PolarPoint{R: r, A: a} +} + +// ToPoint converts a PolarPoint to its Cartesian Point representation. +func (pp PolarPoint) ToPoint() Point { + // TODO: Is noramlizing the right thing to do? + pp.A = NormalizeRadians(pp.A) // range [-Pi,+Pi] + return Point{ + X: pp.R * Meters(math.Cos(float64(pp.A))), + Y: pp.R * Meters(math.Sin(float64(pp.A))), + } +} diff --git a/goverdrive/phys/phys_test.go b/goverdrive/phys/phys_test.go new file mode 100644 index 0000000..94fc331 --- /dev/null +++ b/goverdrive/phys/phys_test.go @@ -0,0 +1,260 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package phys + +import ( + "math" + "testing" +) + +const ( + mTol Meters = 1.0e-6 // XXX: Need more than micrometer-level precision? + rTol Radians = 1.0e-6 // XXX: not sure how much precision is needed for goverdrive +) + +////////////////////////////////////////////////////////////////////// + +type nearTestVec struct { + m1 Meters + m2 Meters + tol Meters + exp bool // expected MetersAreNear() result +} + +func TestMetersAreNear(t *testing.T) { + testTable := []nearTestVec{ + nearTestVec{m1: +0.00, m2: +0.10, tol: 0.05, exp: false}, + nearTestVec{m1: +0.00, m2: +0.10, tol: 0.10, exp: true}, + nearTestVec{m1: +0.00, m2: +0.10, tol: 0.20, exp: true}, + nearTestVec{m1: -0.00, m2: +0.00, tol: 0.00, exp: true}, + nearTestVec{m1: -0.04, m2: +0.05, tol: 0.10, exp: true}, + nearTestVec{m1: +0.04, m2: -0.05, tol: 0.10, exp: true}, + nearTestVec{m1: -0.05, m2: +0.05, tol: 0.10, exp: true}, + nearTestVec{m1: -0.06, m2: +0.06, tol: 0.10, exp: false}, + nearTestVec{m1: +0.10, m2: +0.19, tol: 0.10, exp: true}, + nearTestVec{m1: +0.10, m2: +0.20, tol: 0.10, exp: true}, + nearTestVec{m1: +0.10, m2: +0.21, tol: 0.10, exp: false}, + nearTestVec{m1: -0.10, m2: -0.19, tol: 0.10, exp: true}, + nearTestVec{m1: -0.10, m2: -0.20, tol: 0.10, exp: true}, + nearTestVec{m1: -0.10, m2: -0.21, tol: 0.10, exp: false}, + nearTestVec{m1: +0.0000001, m2: +0.00000011, tol: 0.0000000110, exp: true}, + nearTestVec{m1: +0.0000001, m2: +0.00000011, tol: 0.0000000101, exp: true}, + nearTestVec{m1: +0.0000001, m2: +0.00000011, tol: 0.0000000090, exp: false}, + } + + for i, vec := range testTable { + got := MetersAreNear(vec.m1, vec.m2, vec.tol) + if got != vec.exp { + t.Errorf("Vec=%d MetersAreNear(%v, %v, %v) mismatch; exp=%v, got=%v", i, vec.m1, vec.m2, vec.tol, vec.exp, got) + } + // swap order of func args + got2 := MetersAreNear(vec.m2, vec.m1, vec.tol) + if got2 != vec.exp { + t.Errorf("Vec=%d MetersAreNear(%v, %v, %v) mismatch; exp=%v, got=%v", i, vec.m2, vec.m1, vec.tol, vec.exp, got2) + } + } +} + +////////////////////////////////////////////////////////////////////// + +type distTestVec struct { + p1 Point + p2 Point + exp Meters +} + +func TestDist(t *testing.T) { + testTable := []distTestVec{ + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: +0.00, Y: +0.00}, exp: 0.00}, + distTestVec{p1: Point{X: -0.00, Y: -0.00}, p2: Point{X: +0.00, Y: +0.00}, exp: 0.00}, + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: +0.10, Y: +0.00}, exp: 0.10}, // on-axis (90 degrees), from origin + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: +0.00, Y: +0.10}, exp: 0.10}, + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: -0.10, Y: +0.00}, exp: 0.10}, + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: +0.00, Y: -0.10}, exp: 0.10}, + distTestVec{p1: Point{X: +0.10, Y: +0.40}, p2: Point{X: +0.20, Y: +0.40}, exp: 0.10}, // on-axis (90 degrees), not from origin + distTestVec{p1: Point{X: +0.20, Y: +0.30}, p2: Point{X: +0.10, Y: +0.30}, exp: 0.10}, + distTestVec{p1: Point{X: +0.30, Y: +0.20}, p2: Point{X: +0.30, Y: +0.10}, exp: 0.10}, + distTestVec{p1: Point{X: +0.40, Y: +0.10}, p2: Point{X: +0.40, Y: +0.20}, exp: 0.10}, + distTestVec{p1: Point{X: +0.10, Y: -0.40}, p2: Point{X: +0.20, Y: -0.40}, exp: 0.10}, // on-axis (90 degrees), not from origin + distTestVec{p1: Point{X: +0.20, Y: -0.30}, p2: Point{X: +0.10, Y: -0.30}, exp: 0.10}, + distTestVec{p1: Point{X: +0.30, Y: -0.20}, p2: Point{X: +0.30, Y: -0.10}, exp: 0.10}, + distTestVec{p1: Point{X: +0.40, Y: -0.10}, p2: Point{X: +0.40, Y: -0.20}, exp: 0.10}, + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: +1.00, Y: +1.00}, exp: Meters(math.Sqrt(2.0))}, // 45 degress, from origin + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: -1.00, Y: +1.00}, exp: Meters(math.Sqrt(2.0))}, + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: +1.00, Y: -1.00}, exp: Meters(math.Sqrt(2.0))}, + distTestVec{p1: Point{X: +0.00, Y: +0.00}, p2: Point{X: -1.00, Y: -1.00}, exp: Meters(math.Sqrt(2.0))}, + } + for i, vec := range testTable { + got := Dist(vec.p1, vec.p2) + if !MetersAreNear(got, vec.exp, mTol) { + t.Errorf("Vec=%d Dist(%v, %v) mismatch; exp=%v, got=%v", i, vec.p1, vec.p2, vec.exp, got) + } + // swap order of func args + got2 := Dist(vec.p2, vec.p1) + if !MetersAreNear(got2, vec.exp, mTol) { + t.Errorf("Vec=%d Dist(%v, %v) mismatch; exp=%v, got=%v", i, vec.p2, vec.p1, vec.exp, got2) + } + } +} + +////////////////////////////////////////////////////////////////////// + +type p2ppTestVec struct { + p Point + pp PolarPoint +} + +// TestPointConv tests conversions between Point and PolarPoint +func TestPointConv(t *testing.T) { + const pi Radians = Radians(math.Pi) // for brevity + + /// Spot-check edge cases, different quadrants, etc + // Point -> PolarPoint + testTable1 := []p2ppTestVec{ + p2ppTestVec{p: Point{X: +00.000000, Y: +00.000000}, pp: PolarPoint{R: +00.000000, A: +0.00 * pi}}, + p2ppTestVec{p: Point{X: +10.000001, Y: +00.000000}, pp: PolarPoint{R: +10.000001, A: +0.00 * pi}}, + p2ppTestVec{p: Point{X: -10.000001, Y: +00.000000}, pp: PolarPoint{R: +10.000001, A: +1.00 * pi}}, + p2ppTestVec{p: Point{X: +00.000000, Y: +10.000001}, pp: PolarPoint{R: +10.000001, A: +0.50 * pi}}, + p2ppTestVec{p: Point{X: -00.000000, Y: -10.000001}, pp: PolarPoint{R: +10.000001, A: -0.50 * pi}}, + p2ppTestVec{p: Point{X: -05.000000, Y: +05.000000}, pp: PolarPoint{R: Meters(5 * math.Sqrt(2.0)), A: pi * 0.75}}, + } + + for i, vec := range testTable1 { + pp := vec.p.ToPolarPoint() + if !MetersAreNear(vec.pp.R, pp.R, mTol) || !RadiansAreNear(vec.pp.A, pp.A, rTol) { + t.Errorf("Vec=%d ToPolarPoint(%s) mismatch; exp=%s, got=%s", i, vec.p.String(), vec.pp.String(), pp.String()) + } + } + + // PolarPoint -> Point + testTable2 := []p2ppTestVec{ + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +0.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +1.00 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +1.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +2.00 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: -2.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: -0.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: -1.00 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: -1.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: -2.00 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: -2.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +0.50 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +0.00 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +10.000001, A: +0.00 * pi}, p: Point{X: +10.000001, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +00.000000, A: +2.00 * pi}, p: Point{X: +00.000000, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +10.000001, A: +2.00 * pi}, p: Point{X: +10.000001, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +08.000002, A: +1.00 * pi}, p: Point{X: -08.000002, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +05.000005, A: +3.00 * pi}, p: Point{X: -05.000005, Y: +00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +06.000006, A: +2.50 * pi}, p: Point{X: +00.000000, Y: +06.000006}}, + p2ppTestVec{pp: PolarPoint{R: +07.000007, A: +3.50 * pi}, p: Point{X: -00.000000, Y: -07.000007}}, + p2ppTestVec{pp: PolarPoint{R: +08.000008, A: +12.0 * pi}, p: Point{X: +08.000008, Y: -00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +08.000008, A: -11.0 * pi}, p: Point{X: -08.000008, Y: -00.000000}}, + p2ppTestVec{pp: PolarPoint{R: +03.000003, A: -11.5 * pi}, p: Point{X: -00.000000, Y: +03.000003}}, + p2ppTestVec{pp: PolarPoint{R: +02.000002, A: -13.5 * pi}, p: Point{X: -00.000000, Y: +02.000002}}, + p2ppTestVec{pp: PolarPoint{R: +02.000002, A: -14.5 * pi}, p: Point{X: -00.000000, Y: -02.000002}}, + } + + for i, vec := range testTable2 { + p := vec.pp.ToPoint() + if !MetersAreNear(vec.p.X, p.X, mTol) || !MetersAreNear(vec.p.Y, p.Y, mTol) { + t.Errorf("Vec=%d ToPoint(%s) mismatch; exp=%s, got=%s", i, vec.pp.String(), vec.p.String(), p.String()) + } + } + + /// Sweep: PolarPoint -> Point -> PolarPoint + for r := 0.1; r < 10.0; r += 0.317 { + for a := (-4 * math.Pi); a < (4 * math.Pi); a += 0.101 { + pp1 := PolarPoint{R: Meters(r), A: Radians(a)} + pp1n := PolarPoint{R: pp1.R, A: NormalizeRadians(pp1.A)} + p := pp1.ToPoint() + pp2 := p.ToPolarPoint() + if !MetersAreNear(pp1n.R, pp2.R, mTol) || !RadiansAreNear(pp1n.A, pp2.A, rTol) { + t.Errorf("PolarPoint->Point->PolarPoint mismatch for %s; exp=%s, got=%s", pp1.String(), pp1n.String(), pp2.String()) + } + } + } +} + +////////////////////////////////////////////////////////////////////// + +type poseTestVec struct { + start Pose + delta Pose + exp Pose +} + +// makePose is for brevity to specify test tables +func makePose(x, y Meters, theta Radians) Pose { + return Pose{Point: Point{X: x, Y: y}, Theta: theta} +} + +func TestAdvancePose(t *testing.T) { + const pi Radians = Radians(math.Pi) // for brevity + + testTable := []poseTestVec{ + // advance by {0,0,0} => no change froms start + poseTestVec{start: makePose(+1.000, +2.000, +0.500*pi), delta: makePose(+0.000, +0.000, +0.000*pi), exp: makePose(+1.000, +2.000, +0.500*pi)}, + poseTestVec{start: makePose(-2.000, +3.000, +0.600*pi), delta: makePose(+0.000, +0.000, +0.000*pi), exp: makePose(-2.000, +3.000, +0.600*pi)}, + poseTestVec{start: makePose(+3.000, -4.000, -0.700*pi), delta: makePose(+0.000, +0.000, +0.000*pi), exp: makePose(+3.000, -4.000, -0.700*pi)}, + poseTestVec{start: makePose(-4.000, -5.000, -0.800*pi), delta: makePose(+0.000, +0.000, +0.000*pi), exp: makePose(-4.000, -5.000, -0.800*pi)}, + // start from origin + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(+0.000, +0.000, +0.000*pi), exp: makePose(+0.000, +0.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+1.000, +0.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(-1.000, +0.000, +0.000*pi), exp: makePose(-1.000, +0.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+0.000, +1.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(+0.000, -1.000, +0.000*pi), exp: makePose(+0.000, -1.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(+0.000, +0.000, +0.900*pi), exp: makePose(+0.000, +0.000, +0.900*pi)}, + poseTestVec{start: makePose(+0.000, +0.000, +0.000*pi), delta: makePose(+0.000, +0.000, -0.900*pi), exp: makePose(+0.000, +0.000, -0.900*pi)}, + // start from non-orgin, theta=0 + poseTestVec{start: makePose(+1.000, +0.000, +0.000*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+2.000, +0.000, +0.000*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.000*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+1.000, +1.000, +0.000*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.000*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+1.000, +0.000, +1.000*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.000*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(+2.000, +2.000, +0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.000*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+1.000, +1.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.000*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+0.000, +2.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.000*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+0.000, +1.000, +1.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.000*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(+1.000, +3.000, +0.500*pi)}, + // start from non-orgin, theta=pi + poseTestVec{start: makePose(+1.000, +0.000, +1.000*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+0.000, +0.000, +1.000*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +1.000*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+1.000, -1.000, +1.000*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +1.000*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+1.000, +0.000, +0.000*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +1.000*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(+0.000, -2.000, -0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +1.000*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(-1.000, +1.000, +1.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +1.000*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+0.000, +0.000, +1.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +1.000*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+0.000, +1.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +1.000*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(-1.000, -1.000, -0.500*pi)}, + // start from non-orgin, theta=(pi/2) + poseTestVec{start: makePose(+1.000, +0.000, +0.500*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+1.000, +1.000, +0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.500*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+0.000, +0.000, +0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.500*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+1.000, +0.000, -0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.500*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(-1.000, +1.000, +1.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.500*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+0.000, +2.000, +0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.500*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(-1.000, +1.000, +0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.500*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+0.000, +1.000, -0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, +0.500*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(-2.000, +2.000, +1.000*pi)}, + // start from non-orgin, theta=(-pi/2) + poseTestVec{start: makePose(+1.000, +0.000, -0.500*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+1.000, -1.000, -0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, -0.500*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+2.000, +0.000, -0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, -0.500*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+1.000, +0.000, +0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, -0.500*pi), delta: makePose(+1.000, +2.000, +0.500*pi), exp: makePose(+3.000, -1.000, +0.000*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, -0.500*pi), delta: makePose(+1.000, +0.000, +0.000*pi), exp: makePose(+0.000, +0.000, -0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, -0.500*pi), delta: makePose(+0.000, +1.000, +0.000*pi), exp: makePose(+1.000, +1.000, -0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, -0.500*pi), delta: makePose(+0.000, +0.000, +1.000*pi), exp: makePose(+0.000, +1.000, +0.500*pi)}, + poseTestVec{start: makePose(+0.000, +1.000, -0.500*pi), delta: makePose(+1.000, -2.000, +0.400*pi), exp: makePose(-2.000, +0.000, -0.100*pi)}, + // theta=(+pi/4) and (-pi/4) + poseTestVec{start: makePose(+0.000, +0.000, +0.250*pi), delta: makePose(+1.000, +1.000, +0.250*pi), exp: makePose(+0.000, Meters(math.Sqrt(2.0)), +0.500*pi)}, + poseTestVec{start: makePose(+1.000, +0.000, +0.250*pi), delta: makePose(+1.000, +1.000, -0.250*pi), exp: makePose(+1.000, Meters(math.Sqrt(2.0)), +0.000*pi)}, + poseTestVec{start: makePose(+1.000, +1.000, +0.250*pi), delta: makePose(-2.0*Meters(math.Sqrt(2.0)), +0.000, -1.000*pi), exp: makePose(-1.000, -1.000, -0.750*pi)}, + poseTestVec{start: makePose(+1.000, -1.000, -0.250*pi), delta: makePose(-2.0*Meters(math.Sqrt(2.0)), +0.000, +0.500*pi), exp: makePose(-1.000, +1.000, +0.250*pi)}, + } + + for i, vec := range testTable { + gotPose := vec.start.AdvancePose(vec.delta) + if !MetersAreNear(gotPose.X, vec.exp.X, mTol) || + !MetersAreNear(gotPose.Y, vec.exp.Y, mTol) || + !RadiansAreNear(gotPose.Theta, vec.exp.Theta, rTol) { + t.Errorf("Vec=%d %s.AdvancePose(%s) mismatch; exp=%s, got=%s", i, vec.start.String(), vec.delta.String(), vec.exp.String(), gotPose.String()) + } + } +} diff --git a/goverdrive/phys/units.go b/goverdrive/phys/units.go new file mode 100644 index 0000000..58fa05c --- /dev/null +++ b/goverdrive/phys/units.go @@ -0,0 +1,88 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package phys provides the foundation for modeling the physical world. For +// example, it defines the units of length and angle, and coordinate types. +package phys + +import ( + "math" +) + +////////////////////////////////////////////////////////////////////// +/// Time +////////////////////////////////////////////////////////////////////// + +// SimTime is the measurement of simulation time. It starts at 0 and increases +// with every Tick. +type SimTime uint64 // nanoseconds + +const ( + SimNanosecond = 1 + SimMicrosecond = 1e3 + SimMillisecond = 1e6 + SimSecond = 1e9 +) + +////////////////////////////////////////////////////////////////////// +/// Physical units +////////////////////////////////////////////////////////////////////// + +// Meters is a unit of distance +type Meters float64 + +// MetersPerSec is a unit of speed and velocity +type MetersPerSec float64 + +// MetersPerSec2 is a unit of acceleration +type MetersPerSec2 float64 + +// Radians is a unit of angle. 360 degrees = 2*pi Radians. +type Radians float64 + +// Grams is a unit of mass +type Grams float32 + +////////////////////////////////////////////////////////////////////// +// Constants +////////////////////////////////////////////////////////////////////// + +const ( + Radians90DegreeTurnR Radians = -(math.Pi / 2) + Radians90DegreeTurnL Radians = +(math.Pi / 2) +) + +////////////////////////////////////////////////////////////////////// +/// Conversions, Comparisons, etc +////////////////////////////////////////////////////////////////////// + +func isNear(v1, v2, tolerance float64) bool { + return math.Abs(v1-v2) <= math.Abs(tolerance) +} + +// MetersAreNear returns true if two Meters values are near each other, within a +// specified tolerance. +func MetersAreNear(m1, m2, tolerance Meters) bool { + return isNear(float64(m1), float64(m2), float64(tolerance)) +} + +// MetersPerSecAreNear returns true if two MetersPerSec values are near each +// other, within a specified tolerance. +func MetersPerSecAreNear(m1, m2, tolerance MetersPerSec) bool { + return isNear(float64(m1), float64(m2), float64(tolerance)) +} + +// RadiansAreNear returns true if two Radians values are near each other, within +// a specified tolerance. +func RadiansAreNear(a1, a2, tolerance Radians) bool { + return isNear(float64(a1), float64(a2), float64(tolerance)) +} + +// NormalizeRadians adjusts a Radians value to be in the range [-Pi, +Pi]. +func NormalizeRadians(a Radians) Radians { + for ; a < (-math.Pi); a += (2 * math.Pi) { + } + for ; a > (+math.Pi); a -= (2 * math.Pi) { + } + return a +} diff --git a/goverdrive/proj_setup.sh b/goverdrive/proj_setup.sh new file mode 100644 index 0000000..c7522f8 --- /dev/null +++ b/goverdrive/proj_setup.sh @@ -0,0 +1,93 @@ +#!/bin/bash +# +# This script sets up a Go work area for the `goverdrive` project, +# starting from an empty base project directory. For example, if +# you put each of your go projects under ~/proj/go/, this script +# could be run frome th sub-directory that houses the project: +# $ cd ~/proj/go +# $ mkdir goverdrive +# $ cp goverdrive +# $ chmod +x goverdrive/proj_setup.sh +# $ cd goverdrive +# $ ./proj_setup.sh + + +###################################################################### +# Setup the Go project area +###################################################################### +mkdir bin pkg src + +echo ' +export GOENV=verdrive +export GOPATH=~/proj/go/verdrive +export PATH=~/proj/go/verdrive/bin:$PATH +' >> bin/activate + +chmod +x bin/activate +. bin/activate + + +###################################################################### +# Get latest source code for goverdrive +###################################################################### +go get github.com/anki/goverdrive + + +###################################################################### +# Get source code for supporting code +# TODO: Use a loop, for God's sake!! +###################################################################### +go get github.com/faiface/glhf +pushd src/github.com/faiface/glhf +git checkout --quiet 98c0391c0fd3f0b365cfe5d467ac162b79dfb002 +popd + +go get github.com/faiface/mainthread +pushd src/github.com/faiface/mainthread +git checkout --quiet 7127dc8993886d1ce799086ff298ff99bf258fbf +popd + +go get github.com/faiface/pixel +pushd src/github.com/faiface/pixel +git checkout --quiet 4792a9ebd80d8ed576dff53e6d9d8a761826d82e +popd + +go get github.com/go-gl/gl/v3.3-core/gl +pushd src/github.com/go-gl/gl/v3.3-core/gl +git checkout --quiet ac0d3d2af0fe995e2bb09fa6619b716b0c0fc0fa +popd + +go get github.com/go-gl/glfw/v3.2/glfw +pushd src/github.com/go-gl/glfw/v3.2/glfw +git checkout --quiet 513e4f2bf85c31fba0fc4907abd7895242ccbe50 +popd + +go get github.com/go-gl/mathgl/mgl32 +pushd src/github.com/go-gl/mathgl/mgl32 +git checkout --quiet 4c3fc6b4bf30179013667e5d0b926e024d9a13c1 +popd + +go get github.com/pkg/errors +pushd src/github.com/pkg/errors +git checkout --quiet 2b3a18b5f0fb6b4f9190549597d3f962c02bc5eb +popd + +go get golang.org/x/image/colornames +pushd src/golang.org/x/image/colornames +git checkout --quiet e20db36d77bd0cb36cea8fe49d5c37d82d21591f +popd + +go get golang.org/x/image/font +pushd src/golang.org/x/image/font +git checkout --quiet e20db36d77bd0cb36cea8fe49d5c37d82d21591f +popd + +go get golang.org/x/image/math/f32 +pushd src/golang.org/x/image/math/f32 +git checkout --quiet e20db36d77bd0cb36cea8fe49d5c37d82d21591f +popd + +go get golang.org/x/image/math/fixed +pushd src/golang.org/x/image/math/fixed +git checkout --quiet e20db36d77bd0cb36cea8fe49d5c37d82d21591f +popd diff --git a/goverdrive/robo/collision.go b/goverdrive/robo/collision.go new file mode 100644 index 0000000..6cd9d32 --- /dev/null +++ b/goverdrive/robo/collision.go @@ -0,0 +1,281 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com +// +// Detect vehicle collisions. There may or may not be a reaction. +// +// TODO(gwenz): Should the robotics system natively support collisions between a +// vehicle and a non-vehicle object, eg a road obstacle that is part of the +// game? + +package robo + +import ( + _ "fmt" + "math" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo/track" +) + +// VehicleCollider monitors vehicles to detect collisions, and possibly change +// vehicle state when a collision happens. +type VehicleCollider interface { + // NewCollisions returns all of the vehicle collisions that have happened + // since the last call to NewCollisions. In most cases, this will be a very + // small list (or empty list). There is no order guarantee for returned + // CollisionEvents. NOTE: If called irregularly, some of the older "new" + // collisions may be forgotten + NewCollisions() []CollisionEvent + + // CurCollisions returns all collision events that are ongoing. There is no + // order guarantee for returned CollisionEvents. + CurCollisions() []CollisionEvent + + // Update updates the collision and vehicle states, based on position of each + // vehicle. Should be called by the robotics system. + update(now phys.SimTime, trk *track.Track, vehs *[]Vehicle) +} + +// VehicleCollisionInfo captures the collision info for one of the two vehicles +// involved. The POI (point-of-impact) is in that vehicle's frame of reference. +type VehicleCollisionInfo struct { + Id int + POI phys.Point +} + +// CollisionEvent captures all of the information about a vehicle's collision +// with another vehicle, at the moment of impact. +type CollisionEvent struct { + ImpactTime phys.SimTime + VehInfo [2]VehicleCollisionInfo +} + +// XXX(gwenz): Angle boundaries for high-level colllision direction are +// simplistic. May want to take each vehicle's actual length and width into +// account. + +func (vci VehicleCollisionInfo) IsFrontCollision() bool { + angle := vci.POI.ToPolarPoint().A + return (angle > (-math.Pi / 4)) && (angle < (+math.Pi / 4)) +} + +func (vci VehicleCollisionInfo) IsRearCollision() bool { + angle := vci.POI.ToPolarPoint().A + return (angle > (3 * math.Pi / 4)) || (angle < (-4 * math.Pi / 4)) +} + +func (vci VehicleCollisionInfo) IsLeftSideCollision() bool { + angle := vci.POI.ToPolarPoint().A + return (angle >= (math.Pi / 4)) && (angle <= (3 * math.Pi / 4)) +} + +func (vci VehicleCollisionInfo) IsRightSideCollision() bool { + angle := vci.POI.ToPolarPoint().A + return (angle >= (-3 * math.Pi / 4)) && (angle <= (-math.Pi / 4)) +} + +////////////////////////////////////////////////////////////////////// + +// CollisionDetector is a simple detector of vehicle collisions, based on +// rectangular vehicle gemoetry. It does not modify vehicle state when a +// collision happens. +type CollisionDetector struct { + maxDimension map[vehPair]phys.Meters + curCollisions map[vehPair]CollisionEvent + newCollisions map[vehPair]CollisionEvent +} + +type vehPair struct { + Veh1, Veh2 int +} + +// NewCollisionDetector creates a new detector suited for the specific trk and +// set of vehicles. +func NewCollisionDetector(trk *track.Track, vehs *[]Vehicle) *CollisionDetector { + maxDimension := make(map[vehPair]phys.Meters) + for v1 := range *vehs { + for v2 := v1 + 1; v2 < len(*vehs); v2++ { + veh1 := (*vehs)[v1] + veh2 := (*vehs)[v2] + lmax := math.Max(float64(veh1.Length()), float64(veh2.Length())) + wmax := math.Max(float64(veh1.Width()), float64(veh2.Width())) + max := math.Max(lmax, wmax) + maxDimension[vehPair{v1, v2}] = phys.Meters(max) + } + } + + return &CollisionDetector{ + maxDimension: maxDimension, + curCollisions: make(map[vehPair]CollisionEvent), + newCollisions: make(map[vehPair]CollisionEvent), + } +} + +func (cd *CollisionDetector) NewCollisions() []CollisionEvent { + events := make([]CollisionEvent, 0) + for _, ce := range cd.newCollisions { + events = append(events, ce) + } + // once reported, the collisions aren't "new" anymore + cd.newCollisions = make(map[vehPair]CollisionEvent) + return events +} + +func (cd *CollisionDetector) CurCollisions() []CollisionEvent { + events := make([]CollisionEvent, 0) + for _, ce := range cd.curCollisions { + events = append(events, ce) + } + return events +} + +func (cd *CollisionDetector) update(now phys.SimTime, trk *track.Track, vehs *[]Vehicle) { + // populate collision inputs, for helper function + inputs := make([]vehCollisionInputs, len(*vehs)) + for i, veh := range *vehs { + inputs[i] = vehCollisionInputs{ + dofs: veh.CurTrackPose().Dofs, + pose: trk.ToPose(veh.CurTrackPose()), + len: veh.Length(), + width: veh.Width(), + } + } + + cd.updateHelper(now, trk, inputs) +} + +////////////////////////////////////////////////////////////////////// + +// vehCollisionInputs is an intermediate type that holds all of the per-vehicle +// information needed to detect vehicles collisions. This is intended to help +// unit test the collision indexing and math without having to create a track +// and set of vehicles and then carefully manipulate their state. +type vehCollisionInputs struct { + dofs phys.Meters // Track + pose phys.Pose // Cartesian + len phys.Meters + width phys.Meters +} + +func (cd *CollisionDetector) updateHelper(now phys.SimTime, trk *track.Track, allInputs []vehCollisionInputs) { + for v0 := range allInputs { + for v1 := v0 + 1; v1 < len(allInputs); v1++ { + pair := vehPair{v0, v1} + + // Track pieces can overlap in 2D space, ie very different Dofs values can + // map to same Cartesian coordinates, such as an overpass. In this case, + // the vehicles are NOT colliding. + maxDim := cd.maxDimension[pair] + if trk.DofsDist(allInputs[v0].dofs, allInputs[v1].dofs) > maxDim { + delete(cd.curCollisions, pair) + continue + } + + // vehicles are close => need to do the collision math + poiInputs := [2]vehCollisionInputs{allInputs[v0], allInputs[v1]} + isCollision, absPOI := calcPointOfImpact(poiInputs) + if isCollision { + if _, ok := cd.curCollisions[pair]; !ok { + // Convert absolute Cartesian point into vehicle-relative point for + // each vehicle + var vehInfo [2]VehicleCollisionInfo + impactPose := phys.Pose{Point: absPOI, Theta: 0} + vehInfo[0].POI = impactPose.RelativeTo(allInputs[v0].pose).Point + vehInfo[1].POI = impactPose.RelativeTo(allInputs[v1].pose).Point + vehInfo[0].Id = v0 + vehInfo[1].Id = v1 + + newEvent := CollisionEvent{ + ImpactTime: now, + VehInfo: vehInfo, + } + cd.curCollisions[pair] = newEvent + cd.newCollisions[pair] = newEvent + // NOTE: ^^^ will quietly replace any existing "newCollision" for the pair + } + // For non-new collisions, do NOT update curCollisions, to preserve the + // initial time of impact. + } else { + // not colliding at this moment + delete(cd.curCollisions, pair) + continue + } + } + } +} + +// calcPointOfImpact determines if two vehicles are colliding, based on their +// physical position and dimensions. If they are colliding, a point-of-impact is +// calculated (absolute Cartesian coordinate space). +// - Not colliding => returns false with invalid phys.Point +// - Colliding => returns true with valid phys.Point +func calcPointOfImpact(inputs [2]vehCollisionInputs) (bool, phys.Point) { + // Collision detect algorithm: + // - A vehicles is modeled as a rectangle + // - Check if any of the four corners of one vehicle is inside the other vehicle + + collisionPoints := make([]phys.Point, 0) + for rv := 0; rv < 2; rv++ { // rv = index of the "Reference" vehicle + ov := (rv + 1) % 2 // ov = index of the "Other" vehicle + + // Abs = calculate the Other vehicle's four corner points, in absolute + // Cartesian frame of reference + ovHalfLen := inputs[ov].len / 2 + ovHalfWid := inputs[ov].width / 2 + ovCornersAbs := []phys.Point{ + inputs[ov].pose.AdvancePose(phys.Pose{Point: phys.Point{X: +ovHalfLen, Y: +ovHalfWid}, Theta: 0}).Point, // front L + inputs[ov].pose.AdvancePose(phys.Pose{Point: phys.Point{X: +ovHalfLen, Y: -ovHalfWid}, Theta: 0}).Point, // front R + inputs[ov].pose.AdvancePose(phys.Pose{Point: phys.Point{X: -ovHalfLen, Y: +ovHalfWid}, Theta: 0}).Point, // back L + inputs[ov].pose.AdvancePose(phys.Pose{Point: phys.Point{X: -ovHalfLen, Y: -ovHalfWid}, Theta: 0}).Point, // back R + } + // for _, corner := range ovCornersAbs { + // fmt.Printf("ov=%v => ovCornersAbs=%s\n", ov, corner.String()) + // } + + // Rel = calculate the Other vehicle's four corner points, in Cartesian + // frame of reference relative to the Reference vehicle + ovCornersRel := make([]phys.Point, 4) + for i, cp := range ovCornersAbs { + cpose := phys.Pose{Point: cp, Theta: 0} + ovCornersRel[i] = cpose.RelativeTo(inputs[rv].pose).Point + } + // for _, corner := range ovCornersRel { + // fmt.Printf("ov=%v => ovCornersRel=%s\n", ov, corner.String()) + // } + + // Determine which of the Other vehicle's four corners are inside the + // Reference vehicle's rectangle + rvHalfLen := inputs[rv].len / 2 + rvHalfWid := inputs[rv].width / 2 + // xstr := fmt.Sprintf("x = [%v %v %v %v %v %v %v %v]", rvHalfLen, rvHalfLen, -rvHalfLen, -rvHalfLen, ovCornersRel[0].X, ovCornersRel[1].X, ovCornersRel[2].X, ovCornersRel[3].X) + // ystr := fmt.Sprintf("y = [%v %v %v %v %v %v %v %v]", rvHalfWid, -rvHalfWid, rvHalfWid, -rvHalfWid, ovCornersRel[0].Y, ovCornersRel[1].Y, ovCornersRel[2].Y, ovCornersRel[3].Y) + // fmt.Printf("%s\n%s\n", xstr, ystr) // XXX: quick-and-dirty for Matlab display + for i, point := range ovCornersRel { + if (point.X > rvHalfLen) || (point.X < -rvHalfLen) || + (point.Y > +rvHalfWid) || (point.Y < -rvHalfWid) { + continue + } + // Note: record the Abs collision point, not Rel + collisionPoints = append(collisionPoints, ovCornersAbs[i]) + //fmt.Printf("ov=%v, corner=%v, collisionPoint=%v\n", ov, i, ovCornersAbs[i]) + } + } + + if len(collisionPoints) == 0 { + return false, phys.Point{X: 0, Y: 0} + } + + // There may be >1 collision point. If so, the "net" collision point (absolute + // Cartesian space) applies to both vehicles and is simply the average of all + // detected collision points. This is not a perfect answer, but is + // straightforward and should be good enough. + collisionPoint := phys.Point{X: 0, Y: 0} + for _, cp := range collisionPoints { + collisionPoint.X += cp.X + collisionPoint.Y += cp.Y + } + collisionPoint.X /= phys.Meters(len(collisionPoints)) + collisionPoint.Y /= phys.Meters(len(collisionPoints)) + + return true, collisionPoint +} diff --git a/goverdrive/robo/collision_test.go b/goverdrive/robo/collision_test.go new file mode 100644 index 0000000..3c0b6d9 --- /dev/null +++ b/goverdrive/robo/collision_test.go @@ -0,0 +1,201 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package robo + +import ( + "fmt" + "math" + "testing" + + "github.com/anki/goverdrive/phys" +) + +////////////////////////////////////////////////////////////////////// + +const ( + nearMTolerance phys.Meters = 1.0e-6 // XXX + nearRTolerance phys.Radians = 1.0e-6 // XXX +) + +// testEqual reports a testing error if the two values are not equal +func testEqual(t *testing.T, tag string, exp interface{}, got interface{}) { + if exp != got { + t.Errorf("%s error: exp=%v, got=%v", tag, exp, got) + } +} + +// testMetersAreNear reports a testing error if the two Meters values are not +// near each other +func testMetersAreNear(t *testing.T, tag string, exp phys.Meters, got phys.Meters) { + if !phys.MetersAreNear(exp, got, nearMTolerance) { + t.Errorf("%s error: exp=%v, got=%v", tag, exp, got) + } +} + +// testRadiansAreNear reports a testing error if the two Radians values are not +// near each other +func testRadiansAreNear(t *testing.T, tag string, exp phys.Radians, got phys.Radians) { + if !phys.RadiansAreNear(exp, got, nearRTolerance) { + t.Errorf("%s error: exp=%v, got=%v", tag, exp, got) + } +} + +////////////////////////////////////////////////////////////////////// + +const ( + // WARNING: Some of the test vectors are hard-coded based on the vehicle + // length and width values. Changing these may break the unit test. + veh0Len = 0.240 + veh0Wid = 0.044 + veh1Len = 0.080 + veh1Wid = 0.040 + + // XXX: These intermediate constants help write test tables + deltaDist = 0.02 + ddd2 = deltaDist / 2 + fals = false // XXX: same width as "true", for uniform-width table text +) + +// XXX(gwenz): Use a flat struct layout so test vectors can be written concisely +type poiTestVec struct { + x1 phys.Meters + y1 phys.Meters + t1 phys.Radians + isCollision bool + poiX phys.Meters + poiY phys.Meters +} + +func (v poiTestVec) String() string { + return fmt.Sprintf("x1=%v y1=%v t1=%v isCollision=%v, poiX=%v, poiY=%v", + v.x1, v.y1, v.t1, v.isCollision, v.poiX, v.poiY) +} + +// calculation helpers, for conscise table entries +// h = half +// w = width +// l = length +// d = diagonal (45 degree) +// p = plus +// m = minus +func hwp() phys.Meters { + return ((veh0Wid + veh1Wid) / 2) + deltaDist +} +func hwm() phys.Meters { + return ((veh0Wid + veh1Wid) / 2) - deltaDist +} +func hlp() phys.Meters { + return ((veh0Len + veh1Len) / 2) + deltaDist +} +func hlm() phys.Meters { + return ((veh0Len + veh1Len) / 2) - deltaDist +} +func hw0m() phys.Meters { + return (veh0Wid / 2) - deltaDist +} +func hl0m() phys.Meters { + return (veh0Len / 2) - deltaDist +} +func hl1d() phys.Meters { + return phys.Meters(float64(veh1Len/2-deltaDist) * math.Sqrt(2)) +} +func hw1d() phys.Meters { + return phys.Meters(float64(veh1Wid/2-deltaDist) * math.Sqrt(2)) +} + +// TestCollisionCalcPointOfImpact tests the calcPointsOfImpact() function, which +// is a helper for collision detection. +func TestCollisionCalcPointOfImpact(t *testing.T) { + // This function has a lot of loops to get good coverage without writing a ton + // of test vectors. + // - testTable is the "base" conditions to check + // - Vehicle 0 is the bigger vehicle and is based at the origin with Theta=0 + // - One set of inner loops translate all coordiantes to different places, + // trying to get good coverage of vehicles in a mix of Cartesian quadrants + // - Another set of inner loops tries permutations of reversing the direction + // the vehicle is facing. (A 180 degree pose change should have same four + // vehicle recangle corners, and hence the same collision point.) + testTable := []poiTestVec{ + // Test two corners of Vehicle 0 are inside Vehicle 1 + // x1 y1 t1 clsn poiX poiY + poiTestVec{+hlp(), 0.0000, 0.0, fals, +0.0000, 0.00000}, + poiTestVec{+hlm(), 0.0000, 0.0, true, +hl0m(), 0.00000}, + poiTestVec{-hlp(), 0.0000, 0.0, fals, +0.0000, 0.00000}, + poiTestVec{-hlm(), 0.0000, 0.0, true, -hl0m(), 0.00000}, + poiTestVec{0.0000, +hwp(), 0.0, fals, +0.0000, 0.00000}, + poiTestVec{0.0000, +hwm(), 0.0, true, +0.0000, +hw0m()}, + poiTestVec{0.0000, -hwp(), 0.0, fals, +0.0000, 0.00000}, + poiTestVec{0.0000, -hwm(), 0.0, true, +0.0000, -hw0m()}, + + // Test one corners of Vehicle 0 is inside Vehicle 1, and vice versa + // x1 y1 t1 clsn poiX poiY + poiTestVec{+hlp(), +hwm(), 0.0, fals, +0.0000 + 0.00, 0.00000 + 0.00}, + poiTestVec{+hlm(), +hwm(), 0.0, true, +hl0m() + ddd2, +hw0m() + ddd2}, + poiTestVec{-hlp(), +hwm(), 0.0, fals, +0.0000 + 0.00, 0.00000 + 0.00}, + poiTestVec{-hlm(), +hwm(), 0.0, true, -hl0m() - ddd2, +hw0m() + ddd2}, + poiTestVec{+hlp(), -hwm(), 0.0, fals, +0.0000 + 0.00, 0.00000 + 0.00}, + poiTestVec{+hlm(), -hwm(), 0.0, true, +hl0m() + ddd2, -hw0m() - ddd2}, + poiTestVec{-hlp(), -hwm(), 0.0, fals, +0.0000 + 0.00, 0.00000 + 0.00}, + poiTestVec{-hlm(), -hwm(), 0.0, true, -hl0m() - ddd2, -hw0m() - ddd2}, + + // Test Vehicle 1 is completely contained in Vehicle 0 + // x1 y1 t1 clsn poiX poiY + poiTestVec{0.0000, 0.0000, 0.0, true, +0.0000 + 0.00, 0.00000 + 0.00}, + poiTestVec{0.0000, 0.0000, 0.2, true, +0.0000 + 0.00, 0.00000 + 0.00}, + + // Test Vehicle rotated 45 degrees, positioned such that exactly one corner + // of Vehicle 0 is inside Vehicle 1 rectangle. (This makes checking the + // collision point easier.) + // XXX(gwenz): This is brittle, and took some effort to get working values. + // x1 y1 t1 clsn poiX poiY + poiTestVec{+veh0Len/2 + hl1d(), +veh0Wid/2 + veh1Wid/2 + hw1d(), 1 * math.Pi / 4, true, +veh0Len / 2, +veh0Wid / 2}, + poiTestVec{-veh0Len/2 - hl1d(), +veh0Wid/2 + veh1Wid/2 + hw1d(), 3 * math.Pi / 4, true, -veh0Len / 2, +veh0Wid / 2}, + poiTestVec{+veh0Len/2 + hl1d(), -veh0Wid/2 - veh1Wid/2 - hw1d(), 3 * math.Pi / 4, true, +veh0Len / 2, -veh0Wid / 2}, + poiTestVec{+veh0Len/2 + hl1d(), -veh0Wid/2 - veh1Wid/2 - hw1d(), 1 * math.Pi / 4, fals, +veh0Len / 2, -veh0Wid / 2}, + poiTestVec{-veh0Len/2 - hl1d(), -veh0Wid/2 - veh1Wid/2 - hw1d(), 1 * math.Pi / 4, true, -veh0Len / 2, -veh0Wid / 2}, + poiTestVec{-veh0Len/2 - hl1d(), -veh0Wid/2 - veh1Wid/2 - hw1d(), 3 * math.Pi / 4, fals, -veh0Len / 2, -veh0Wid / 2}, + } + + for i, vec := range testTable { + vecStr := fmt.Sprintf("Vec %d: poiTestVec=%s", i, vec.String()) + + for _, rad := range []phys.Meters{0, 0.01, 0.1, 1.0} { + for rho := float64(0); rho < (2 * math.Pi); rho += (math.Pi / 4) { + xlatePoint := phys.PolarPoint{R: rad, A: phys.Radians(rho)}.ToPoint() + // translate both vehicle poses by PolarPoint{rad, rho} + vehPose := [2]phys.Pose{ + phys.Pose{Point: phys.Point{X: 0.0000, Y: 0.0000}, Theta: 0.0000}, + phys.Pose{Point: phys.Point{X: vec.x1, Y: vec.y1}, Theta: vec.t1}, + } + for j := range vehPose { + vehPose[j].X += xlatePoint.X + vehPose[j].Y += xlatePoint.Y + } + // translate expected point-of-impact by PolarPoint{rad, rho} + expPoiX := vec.poiX + xlatePoint.X + expPoiY := vec.poiY + xlatePoint.Y + + // rotate each vehicle by +/- 180 degrees => should not affect the result + for dt0 := -1; dt0 < 2; dt0++ { + for dt1 := -1; dt1 < 2; dt1++ { + // test vector -> function input type + vehPose[0].Theta = phys.NormalizeRadians(phys.Radians(math.Pi*float64(dt0)) + vehPose[0].Theta) + vehPose[1].Theta = phys.NormalizeRadians(phys.Radians(math.Pi*float64(dt1)) + vehPose[1].Theta) + inputs := [2]vehCollisionInputs{ + vehCollisionInputs{dofs: 0, pose: vehPose[0], len: veh0Len, width: veh0Wid}, + vehCollisionInputs{dofs: 0, pose: vehPose[1], len: veh1Len, width: veh1Wid}, + } // Note: Dofs ^^^^ is unused + + isCollision, poi := calcPointOfImpact(inputs) + testEqual(t, fmt.Sprintf("%s isCollision", vecStr), vec.isCollision, isCollision) + if isCollision { + testMetersAreNear(t, fmt.Sprintf("%s PointOfImpact.X", vecStr), expPoiX, poi.X) + testMetersAreNear(t, fmt.Sprintf("%s PointOfImpact.Y", vecStr), expPoiY, poi.Y) + } + } + } + } + } + } +} diff --git a/goverdrive/robo/light/light.go b/goverdrive/robo/light/light.go new file mode 100644 index 0000000..3d574da --- /dev/null +++ b/goverdrive/robo/light/light.go @@ -0,0 +1,246 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package light supports lights for an individual vehicle. +// +// Note that light do NOT have any dependency on the visualization system code. +// The light package is designed to not depend on the vizualization. +package light + +import ( + "fmt" + "golang.org/x/image/colornames" + "image/color" + + "github.com/anki/goverdrive/phys" +) + +const ( + RepeatForever = -1 +) + +// Position is the position and size of a "point" light on a vehicle. Position +// is relative to vehicle's center: +// X>0 means towards vehicle front +// Y>0 means towards vehicle left side +// R = radius +type Position struct { + X phys.Meters + Y phys.Meters + R phys.Meters +} + +// Group is a group of lights have the same color at any time, even though they +// may be located in different positions on the vehicle. For example, OverDrive +// Gen2 vehicles have two separate "gun" lights that share one hardware control +// signal. +type Group struct { + defColor color.Color + lights []Position +} + +// Spec is the spec for the full set of lights for the vehicle. The +// handle to each light group in the set is a string name. +type Spec map[string]Group + +// Gen2Spec is matches the real lights on OverDrive (Gen2) vehicle hardware. +var Gen2Spec = Spec{ + "top": Group{defColor: colornames.Black, + lights: []Position{ + Position{X: 0, Y: 0, R: 0.01}, + }}, + "guns": Group{defColor: colornames.Goldenrod, + lights: []Position{ + Position{X: 0.035, Y: -0.015, R: 0.005}, + Position{X: 0.035, Y: +0.015, R: 0.005}, + }}, + "tail": Group{defColor: colornames.Darkred, + lights: []Position{ + Position{X: -0.034, Y: -0.015, R: 0.006}, + Position{X: -0.034, Y: -0.009, R: 0.006}, + Position{X: -0.034, Y: +0.009, R: 0.006}, + Position{X: -0.034, Y: +0.015, R: 0.006}, + }}, +} + +// HexPodSpec is a hexagon pod with a center light, ie seven lights total. The +// pod lights are enumerated is counter-clockwise order, starting with the +// "forward" light. +// +// NOTE: Presently, this light configuration does not exist with real hardware. +var HexPodSpec = Spec{ + "center": Group{defColor: colornames.Black, lights: []Position{Position{X: +0.0000, Y: +0.0000, R: 0.006}}}, + "h0": Group{defColor: colornames.Black, lights: []Position{Position{X: +0.0150, Y: +0.0000, R: 0.006}}}, + "h1": Group{defColor: colornames.Black, lights: []Position{Position{X: +0.0076, Y: +0.0106, R: 0.006}}}, + "h2": Group{defColor: colornames.Black, lights: []Position{Position{X: -0.0076, Y: +0.0106, R: 0.006}}}, + "h3": Group{defColor: colornames.Black, lights: []Position{Position{X: -0.0150, Y: +0.0000, R: 0.007}}}, + "h4": Group{defColor: colornames.Black, lights: []Position{Position{X: -0.0076, Y: -0.0106, R: 0.006}}}, + "h5": Group{defColor: colornames.Black, lights: []Position{Position{X: +0.0076, Y: -0.0106, R: 0.006}}}, +} + +////////////////////////////////////////////////////////////////////// + +// Frame is a single "frame" of an animation for a single light +type Frame struct { + Color color.Color + Tms uint // duration, in milliseconds +} + +// GroupFrame is a "frame" of an animation for a group of independent lights +type GroupFrame struct { + Colors []color.Color + Tms uint // duration, in milliseconds +} + +// animation has frames and internal state to play an animation on a single +// light +type animation struct { + frames []Frame + curFrame int + frameEndTime phys.SimTime + countLeft int +} + +// VehLights has the physical spec and state of the set of lights for one +// vehicle. +type VehLights struct { + spec Spec + static map[string]color.Color // light name -> color + anim map[string]*animation // light name -> animation + cur map[string]color.Color // light name -> color +} + +// VehLightState has all of the information needed to visualize one point light. +type VizInfo struct { + Position + Color color.Color +} + +// NewVehLights creates a usable VehLights object based on a spec. +func NewVehLights(spec Spec) *VehLights { + // static lights start with default colors + static := make(map[string]color.Color) + cur := make(map[string]color.Color) + for k, v := range spec { + static[k] = v.defColor + cur[k] = v.defColor + } + return &VehLights{ + spec: spec, + static: static, + anim: make(map[string]*animation), + cur: cur, + } +} + +func (vl *VehLights) validateName(name string) { + if _, ok := vl.spec[name]; !ok { + panic(fmt.Sprintf("VehLights.Set(%v) failed, light name not recognized", name)) + } +} + +// Set sets the static color of a light group. This cancels any ongoing +// animation for the light. +func (vl *VehLights) Set(name string, color color.Color) { + vl.validateName(name) + vl.anim[name] = nil + vl.static[name] = color +} + +// SetAnimation starts animation of one or more "frames" for a single light. The +// animation is repeated a programmable number of times, or indefinitely if +// repeatCount=RepeatForever. +func (vl *VehLights) SetAnimation(now phys.SimTime, name string, frames []Frame, repeatCount int) { + vl.validateName(name) + if len(frames) == 0 { + panic("SetAnimation with len(frames)=0 is invalid") + } + vl.anim[name] = startAnimation(now, frames, repeatCount) +} + +// SetGroupAnimation starts animation of one or more "frames" for a group of +// independent lights. The animation is repeated a programmable number of times, +// or indefinitely if repeatCount=RepeatForever. +func (vl *VehLights) SetGroupAnimation(now phys.SimTime, names []string, gframes []GroupFrame, repeatCount int) { + if len(gframes) == 0 { + panic("SetGroupAnimation with len(gframes)=0 is invalid") + } + for l, name := range names { + vl.validateName(name) + frames := make([]Frame, len(gframes)) + for i := range gframes { + frames[i].Color = gframes[i].Colors[l] + frames[i].Tms = gframes[i].Tms + } + vl.anim[name] = startAnimation(now, frames, repeatCount) + } +} + +// IsAnimating returns true if a named light has an ongoing animation. +func (vl *VehLights) IsAnimating(name string) bool { + vl.validateName(name) + return vl.anim[name] != nil +} + +// Update updates all light animations and determines the current color of each +// light. +func (vl *VehLights) Update(now phys.SimTime) { + // update light animation and choose final color value for each light + for name, _ := range vl.spec { + vl.cur[name] = vl.static[name] + // animations take precedence over static color value + if anim := vl.anim[name]; anim != nil { + vl.cur[name] = anim.updateAnimation(now) + if anim.isDone() { + vl.anim[name] = nil + vl.cur[name] = vl.static[name] + } + } + } +} + +// VizInfo returns the info to visuzlize for each individual point light of the +// vehicle. +func (vl *VehLights) VizInfo() []*VizInfo { + vizinfo := make([]*VizInfo, 0) + for name, color := range vl.cur { + for _, lp := range vl.spec[name].lights { + vizinfo = append(vizinfo, &VizInfo{Position: Position{X: lp.X, Y: lp.Y, R: lp.R}, Color: color}) + } + } + return vizinfo +} + +func startAnimation(now phys.SimTime, frames []Frame, repeatCount int) *animation { + return &animation{ + frames: frames, + curFrame: 0, + frameEndTime: now + (phys.SimTime(frames[0].Tms) * phys.SimMillisecond), + countLeft: repeatCount, + } +} + +func (a *animation) isDone() bool { + return a.countLeft == 0 +} + +// updateAnimation advances the animation based on the current sim time, and +// returns the updated light color. +func (a *animation) updateAnimation(now phys.SimTime) color.Color { + if a.isDone() { + return color.RGBA{0, 0, 0, 0} + } + + for now >= a.frameEndTime { + // frame is done => next frame + a.curFrame++ + if a.curFrame >= len(a.frames) { + a.curFrame = 0 + if a.countLeft > 0 { // <0 means repeat forever + a.countLeft-- + } + } + a.frameEndTime += (phys.SimTime(a.frames[a.curFrame].Tms) * phys.SimMillisecond) + } + return a.frames[a.curFrame].Color +} diff --git a/goverdrive/robo/sim.go b/goverdrive/robo/sim.go new file mode 100644 index 0000000..8d0c5d9 --- /dev/null +++ b/goverdrive/robo/sim.go @@ -0,0 +1,129 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package robo + +import ( + _ "fmt" + "math" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo/track" +) + +// Simulator simulates all vehicle robotic movement and interaction, such as: +// - Steady-state driving +// - Speed changes +// - Horziontal offset changes +// - Collisions +type Simulator interface { + // Tick updates the state of all vehicles on the track, including speed, pose, + // etc. + Tick(dt phys.SimTime, trk *track.Track, vehs *[]Vehicle) +} + +////////////////////////////////////////////////////////////////////// + +// IdealSimulator simulates ideal motion with no limits to motor ability, no +// center offset drift, etc. It is intended to be very simple. +type IdealSimulator struct { + // TODO: Any fields needed? +} + +func NewIdealSimulator() *IdealSimulator { + return &IdealSimulator{} +} + +func (sim *IdealSimulator) Tick(dt phys.SimTime, trk *track.Track, vehs *[]Vehicle) { + for v, _ := range *vehs { + var veh *Vehicle = &(*vehs)[v] + rpi, _ := trk.RpiAndRpDofs(veh.CurTrackPose().Dofs) + rp := trk.Rp(rpi) + + // To reduce clutter, use type float64 for all intermediate values + fdt := float64(dt) * 1e-9 + desDspd := float64(veh.desDspd) + cmdDspd := float64(veh.cmdDspd) + + // Calc new dofs speed (ie apply constant [de/a]cceleration) + dspdDelta := fdt * float64(veh.cmdDacl) + if math.Abs(desDspd-cmdDspd) <= dspdDelta { + desDspd = cmdDspd + } else if desDspd < cmdDspd { + desDspd += dspdDelta + } else { // desDspd > cmdDspd + desDspd -= dspdDelta + } + curDspd := desDspd // ideal sim model means (cur==des) always + + // Calc new dofs + // Formula = standard calculus for rigid body movement under constant acceleration + deltaFwd := (curDspd * fdt) + ((float64(veh.cmdDacl) / 2) * fdt * fdt) + deltaDofs := deltaFwd + if rp.CurveRadius(0) != 0 { + // remember that Dofs is measured along road center + deltaDofs *= float64(rp.CurveRadius(0)) / float64(rp.CurveRadius(veh.CurTrackPose().Cofs)) + } + + // Calc new hofs + if veh.cmdCofs < -trk.MaxCofs() { + veh.cmdCofs = -trk.MaxCofs() + } else if veh.cmdCofs > trk.MaxCofs() { + veh.cmdCofs = trk.MaxCofs() + } + desCofs := float64(veh.desCofs) + cmdCofs := float64(veh.cmdCofs) + curCspd := math.Abs(float64(veh.cmdCspd)) + curHvel := curCspd + maxDeltaCofs := fdt * curCspd // max possible (for this tick) + absDeltaCofs := float64(0) // actual + if desCofs < cmdCofs { + curHvel = curCspd + if (desCofs + maxDeltaCofs) > cmdCofs { + absDeltaCofs = cmdCofs - desCofs + desCofs = cmdCofs + } else { + absDeltaCofs = maxDeltaCofs + desCofs += maxDeltaCofs + } + } else if desCofs > cmdCofs { + curHvel = -curCspd + if (desCofs - maxDeltaCofs) < cmdCofs { + absDeltaCofs = desCofs - cmdCofs + desCofs = cmdCofs + } else { + absDeltaCofs = maxDeltaCofs + desCofs -= maxDeltaCofs + } + } else { + curHvel = 0 + } + //fmt.Printf(" desCofs=%v, curCspd=%v, absDeltaCofs=%v\n", desCofs, curCspd, absDeltaCofs) + + // Update the vehicle's state + veh.desDspd = phys.MetersPerSec(desDspd) + veh.desCofs = phys.Meters(desCofs) + if veh.IsFacingTrackwise() { + veh.curVel.D = phys.MetersPerSec(curDspd) + veh.curPose.Dofs += phys.Meters(deltaDofs) + } else { + veh.curVel.D = -phys.MetersPerSec(curDspd) + veh.curPose.Dofs -= phys.Meters(deltaDofs) + } + veh.curPose.Dofs = trk.NormalizeDofs(veh.curPose.Dofs) + veh.curPose.Cofs = phys.Meters(desCofs) // ideal sim model means (cur==des) always + veh.curVel.C = phys.MetersPerSec(curHvel) + + // Update pose angle based on new V and H speeds + if curDspd > 0 { + //fmt.Printf("veh.curVel.C=%v, veh.curVel.D=%v\n", veh.curVel.C, veh.curVel.D) + angle := math.Atan2(float64(veh.curVel.C), float64(veh.curVel.D)) + veh.curPose.DAngle = phys.Radians(angle) + } + //fmt.Printf(" veh.curVel.D=%v\n", veh.curVel.D) + + // Update Odometer + pathLen := math.Sqrt((deltaFwd * deltaFwd) + (absDeltaCofs * absDeltaCofs)) + veh.odom += phys.Meters(pathLen) + } +} diff --git a/goverdrive/robo/system.go b/goverdrive/robo/system.go new file mode 100644 index 0000000..176e415 --- /dev/null +++ b/goverdrive/robo/system.go @@ -0,0 +1,61 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package robo is all of the native robotics, such as tracks, vehicles, and +// simulation modeling. +package robo + +import ( + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo/track" +) + +const ( + // TODO(gwenz): It's unclear what range of values works for simDeltaT, and + // whether there is any need to change it. + // Note that this value interacts with roboTicksPerGameTick in gameloop.go. + // They may need to be changed together. + simDeltaT = 1e7 * phys.SimNanosecond +) + +// System is the complete robotics system being simulated. +type System struct { + dt phys.SimTime // length of time for one sim tick + now phys.SimTime + Track track.Track + Vehicles []Vehicle + Collider VehicleCollider + sim Simulator +} + +func NewSystem(trk *track.Track, vehs *[]Vehicle, sim Simulator, collider VehicleCollider) *System { + return &System{ + dt: simDeltaT, + now: 0, + Track: *trk, + Vehicles: *vehs, + Collider: collider, + sim: sim, + } +} + +func (s *System) SimDeltaT() phys.SimTime { + return s.dt +} + +func (s *System) Now() phys.SimTime { + return s.now +} + +// Tick runs the robotics system (vehicle simulation, collision detection, etc) +// for one small "tick" of time, ie a duration of s.dt. Only the game loop +// should call Tick. +func (s *System) Tick() { + s.now += s.dt + s.sim.Tick(s.dt, &s.Track, &s.Vehicles) + for _, v := range s.Vehicles { + v.Lights().Update(s.now) + } + s.Collider.update(s.now, &s.Track, &s.Vehicles) + // TODO: Update/apply external forces? +} diff --git a/goverdrive/robo/track/coord.go b/goverdrive/robo/track/coord.go new file mode 100644 index 0000000..f64dc4d --- /dev/null +++ b/goverdrive/robo/track/coord.go @@ -0,0 +1,55 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package track + +import ( + "github.com/anki/goverdrive/phys" +) + +const ( + // TrackDistTol is the general tolerance for checking if two Meters values are + // equal, in a track point context. + TrackMetersAreEqualTol phys.Meters = 1e-6 + TrackRadiansAreEqualTol phys.Radians = 1e-3 +) + +// Point is a position in the track coordinate space. +// - Is absolute, ie it is independent of driving direction +// - The distance offset is cyclical, like the angle for Polar Coordinates +// - Is non-Euclidean, because points "bend" to match the shape of the track +// - Dofs = distance offset = trackwise distance from finish line, along road center +// - Cofs = center offset, ie from road center +// - In trackwise driving direction, Cofs>0 means left-of-road-center +// - NOTE: "trackwise" is like "clockwise", and means natural forward direction, +// as determined by the start piece (for modular tracks). Counter-trackwise +// means driving opposite of the natural forward direction of the track. +// +// Conversions: +// - track.Point -> phys.Point = straightforward +// - phys.Point -> track.Point = not as straightforward +type Point struct { + Dofs phys.Meters // vert offset + Cofs phys.Meters // horz offset +} + +// Pose adds an orientation (front of vehicle) to a track position +// 0 = facing the track's forward direction AT THAT PARTICULAR Point +// pi = opposite the track's forward direction AT THAT PARTICULAR Point +// >0 = left (ccw rotation) +// <0 = right (cw rotation) +// More generally: +// ABS(DAngle) <= (pi/2) ==> facing trackwise +// (pi/2) < ABS(DAngle) <= (pi) ==> facing counter-trackwise +// ABS(DAngle) > pi ==> undefined (bad things may happen) +type Pose struct { + Point + DAngle phys.Radians +} + +// Vel is the velocity vector of an object, in track coordinate system. D<0 +// means vehicle is moving counter-trackwise. +type Vel struct { + D phys.MetersPerSec // velocity of distance offset (from finish line) + C phys.MetersPerSec // velocity of center offset (from road center) +} diff --git a/goverdrive/robo/track/roadpiece.go b/goverdrive/robo/track/roadpiece.go new file mode 100644 index 0000000..21e6b8f --- /dev/null +++ b/goverdrive/robo/track/roadpiece.go @@ -0,0 +1,95 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package track + +import ( + "fmt" + "math" + + "github.com/anki/goverdrive/phys" +) + +// RoadPiece is the helper type used to define a track +// - Straight or curved only; no fancy shapes like intersection or 'Y' +// - Any anglular change through the piece is along a circular arc +// - Maximum of angular change of +/- pi/2 radians (ie 90 degrees) +// - Width is a parameter of the track, not individual road pieces +type RoadPiece struct { + cenLen phys.Meters // path length, at road center + dAngle phys.Radians // delta angle when driving through the piece (0=>straight; +pi/2=>left turn; etc) +} + +func NewRoadPiece(cenLen phys.Meters, dAngle phys.Radians) *RoadPiece { + if cenLen <= 0 { + panic(fmt.Sprintf("RoadPiece requires cenLen > 0; actual value is %v", cenLen)) + } + if dAngle > phys.Radians90DegreeTurnL || dAngle < phys.Radians90DegreeTurnR { + panic(fmt.Sprintf("RoadPiece requires (%v <= DAngle <= %v); actual value is %v", phys.Radians90DegreeTurnR, phys.Radians90DegreeTurnL, dAngle)) + } + return &RoadPiece{cenLen: cenLen, dAngle: dAngle} +} + +func (rp *RoadPiece) String() string { + return fmt.Sprintf("RoadPice{cenLen: %v, dAngle: %v}", rp.cenLen, rp.dAngle) +} + +func (rp *RoadPiece) CenLen() phys.Meters { + return rp.cenLen +} + +func (rp *RoadPiece) DAngle() phys.Radians { + return rp.dAngle +} + +func (rp *RoadPiece) IsStraight() bool { + return rp.dAngle == 0 +} + +// Len computes the path length of the road piece, at the specified center +// offset. +func (rp *RoadPiece) Len(cofs phys.Meters) phys.Meters { + d := rp.cenLen + d -= cofs * phys.Meters(rp.dAngle) + return d +} + +// CurveRadius computes the radius of the road piece, at the specified center +// offset. Straight pieces, which have no curvature, will return 0. +func (rp *RoadPiece) CurveRadius(cofs phys.Meters) phys.Meters { + if rp.IsStraight() { + return 0 + } + r := rp.cenLen / phys.Meters(math.Abs(float64(rp.dAngle))) + if rp.dAngle < 0 { + // curve right + r += cofs + } else { + // curve left + r -= cofs + } + return r +} + +// DeltaPose returns the change in pose when travelling through the piece, +// assuming the canonical starting pose: origin, facing right. +func (rp *RoadPiece) DeltaPose() phys.Pose { + if rp.IsStraight() { + return phys.Pose{Point: phys.Point{X: rp.cenLen, Y: 0}, Theta: 0} + } + + rc := rp.CurveRadius(0) + angle := phys.Radians(rp.cenLen / rc) + if rp.dAngle > 0 { + // left turn + pp := phys.PolarPoint{R: rc, A: phys.Radians(-(math.Pi / 2) + angle)} + p := pp.ToPoint() + p.Y += rc + return phys.Pose{Point: p, Theta: rp.dAngle} + } + // else: right turn + pp := phys.PolarPoint{R: rc, A: phys.Radians(+(math.Pi / 2) - angle)} + p := pp.ToPoint() + p.Y -= rc + return phys.Pose{Point: p, Theta: rp.dAngle} +} diff --git a/goverdrive/robo/track/roadpiece_test.go b/goverdrive/robo/track/roadpiece_test.go new file mode 100644 index 0000000..ed00399 --- /dev/null +++ b/goverdrive/robo/track/roadpiece_test.go @@ -0,0 +1,159 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package track + +import ( + "math" + "testing" + + "github.com/anki/goverdrive/phys" +) + +////////////////////////////////////////////////////////////////////// + +// TestStraights creates straight road pieces and checks their properties +func TestStraights(t *testing.T) { + straightCenLens := []phys.Meters{ + 0.001, + 0.020, + 0.300, + 4.000, + } + for _, cenLen := range straightCenLens { + rp := NewRoadPiece(cenLen, phys.Radians(0)) + + if rp.CenLen() != cenLen { + t.Errorf("rp.CenLen()=%v is wrong. Exp=%v", rp.CenLen(), cenLen) + } + if rp.DAngle() != 0 { + t.Errorf("rp.DAngle()=%v is wrong. Exp=%v", rp.DAngle(), 0) + } + if !rp.IsStraight() { + t.Errorf("rp.IsStraight()=%v is wrong. Exp=true", rp.IsStraight()) + } + + expDp := phys.Pose{Point: phys.Point{X: cenLen, Y: 0}, Theta: 0} + gotDp := rp.DeltaPose() + if gotDp != expDp { + t.Errorf("rp.DeltaPose()=%v is wrong for rp=%v. Exp=%v", gotDp, rp, expDp) + } + + // all hofs values should have same length and radius of curvature + var hofs phys.Meters + for hofs = -1.1; hofs <= 1.2; hofs += 0.1 { + len := rp.Len(hofs) + if len != cenLen { + t.Errorf("rp.Len(%v)=%v is wrong. Exp=%v", hofs, len, cenLen) + } + cr := rp.CurveRadius(hofs) + if cr != 0 { + t.Errorf("rp.CurveRadius()=%v is wrong. Exp=0", cr) + } + } + } +} + +// TestRightAngleCurves creates 90-degree road pieces and checks their +// properties +func TestRightAngleCurves(t *testing.T) { + for i := 0; i < 10; i++ { // sweep curve radius values + radius := phys.Meters(float64(i+1) * 0.1) // curve radius (at road center) + cenLen := phys.Meters(math.Pi/2) * radius + for j := 0; j < 2; j++ { + // 0 => right turn; 1 => left turn + dAngle := phys.Radians(-math.Pi/2 + float64(j)*math.Pi) + rp := NewRoadPiece(cenLen, dAngle) + + if rp.CenLen() != cenLen { + t.Errorf("rp.CenLen()=%v is wrong for rp=%v. Exp=%v", rp.CenLen(), rp, cenLen) + } + if rp.DAngle() != dAngle { + t.Errorf("rp.DAngle()=%v is wrong for rp=%v. Exp=%v", rp.DAngle(), rp, dAngle) + } + if rp.IsStraight() { + t.Errorf("rp.IsStraight()=%v is wrong for rp=%v. Exp=false", rp.IsStraight(), rp) + } + + expDp := phys.Pose{Point: phys.Point{X: radius, Y: radius}, Theta: rp.DAngle()} + if rp.DAngle() < 0 { // right turn + expDp.Y = -radius + } + gotDp := rp.DeltaPose() + if !phys.RadiansAreNear(gotDp.Theta, expDp.Theta, nearRTolerance) || + !phys.MetersAreNear(gotDp.X, expDp.X, nearMTolerance) || + !phys.MetersAreNear(gotDp.Y, expDp.Y, nearMTolerance) { + t.Errorf("rp.DeltaPose()=%v is wrong for rp=%v. Exp=%v", gotDp, rp, expDp) + } + + // radius of curvature and path len depend on the horizontal offset + hofsList := []phys.Meters{ + +0.00, // road center + +0.07, // left-of-center + -0.05, // right-of-center + +radius, + -radius, + } + for _, hofs := range hofsList { + hofsRadius := radius + hofs // default: right turn + if rp.DAngle() > 0 { + // left turn => larger (+hofs) means smaller curve radius + hofsRadius = radius - hofs + } + expLen := phys.Meters(math.Pi/2) * hofsRadius + gotLen := rp.Len(hofs) + if !phys.MetersAreNear(gotLen, expLen, nearMTolerance) { + t.Errorf("rp.Len(%v)=%v is wrong for rp=%v. Exp=%v", hofs, gotLen, rp, expLen) + } + gotCr := rp.CurveRadius(hofs) + if !phys.MetersAreNear(gotCr, hofsRadius, nearMTolerance) { + t.Errorf("r.CurveRadius(%v)=%v is wrong for rp=%v. Exp=%v", hofs, gotCr, rp, hofsRadius) + } + } + } + } +} + +// TestOtherCurves creates curved road pieces that are NOT 90 degrees turns, and +// checks their properties. +func TestOtherCurves(t *testing.T) { + rpList := make([]RoadPiece, 4, 4) + rpList[0] = *NewRoadPiece(phys.Meters(1.0), phys.Radians(+2.0*math.Pi/6)) // L 60 Degrees + rpList[1] = *NewRoadPiece(phys.Meters(0.5), phys.Radians(+1.0*math.Pi/6)) // L 30 Degrees + rpList[2] = *NewRoadPiece(phys.Meters(0.4), phys.Radians(-1.0*math.Pi/6)) // R 30 Degrees + rpList[3] = *NewRoadPiece(phys.Meters(0.8), phys.Radians(-2.0*math.Pi/6)) // R 60 Degrees + + for _, rp := range rpList { + if rp.IsStraight() { + t.Errorf("rp.IsStraight()=%v is wrong. Exp=false", rp.IsStraight()) + } + expCr := rp.CenLen() / phys.Meters(math.Abs(float64(rp.DAngle()))) + gotCr := rp.CurveRadius(0) + if !phys.MetersAreNear(gotCr, expCr, nearMTolerance) { + t.Errorf("rp.CurveRadius(0)=%v is wrong for rp=%v. Exp=%v", gotCr, rp, expCr) + } + } + + for i := 0; i < 2; i++ { + rp1 := rpList[2*i] + rp2 := rpList[2*i+1] + cr1 := rp1.CurveRadius(0) + cr2 := rp2.CurveRadius(0) + if !phys.MetersAreNear(cr1, cr2, nearMTolerance) { + t.Errorf("rp1.CurveRadius(0)=%v does not equal rp2.CurveRadius(0)=%v", cr1, cr2) + } + + expDeltaPose := phys.Pose{Point: phys.Point{X: cr1, Y: cr1}, Theta: phys.Radians(math.Pi / 2)} + if rp1.DAngle() < 0 { + expDeltaPose.Y = -expDeltaPose.Y + expDeltaPose.Theta = -expDeltaPose.Theta + } + gotDeltaPose := rp1.DeltaPose().AdvancePose(rp2.DeltaPose()) + if !phys.MetersAreNear(gotDeltaPose.X, expDeltaPose.X, nearMTolerance) || + !phys.MetersAreNear(gotDeltaPose.Y, expDeltaPose.Y, nearMTolerance) || + !phys.RadiansAreNear(gotDeltaPose.Theta, expDeltaPose.Theta, nearRTolerance) { + t.Errorf("rp.DeltaPose()=%v is wrong for rp=%v + rp=%v. Exp=%v", gotDeltaPose, rp1, rp2, expDeltaPose) + } + + } +} diff --git a/goverdrive/robo/track/track.go b/goverdrive/robo/track/track.go new file mode 100644 index 0000000..a2d92fc --- /dev/null +++ b/goverdrive/robo/track/track.go @@ -0,0 +1,377 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package track represents a physical track, which is comprised of road pieces. +// It also defines an internal coordinate system, track regions, and other +// concepts to help express game design concepts elegantly. +package track + +import ( + "fmt" + "math" + + "github.com/anki/goverdrive/phys" +) + +const ( + // TrackLenMod... are parameters for standard OverDrive modular tracks + TrackLenModStartLong phys.Meters = 0.34 + TrackLenModStartShort phys.Meters = 0.22 + TrackLenModStraight phys.Meters = 0.56 + TrackLenModCurve phys.Meters = phys.Meters(math.Pi/2) * (TrackLenModStraight / 2) +) + +// Rpi means Road Piece Index, ie a unique index for each road piece of the +// track. An Rpi value that <0 means invalid or not found, depending on the +// context. +type Rpi int + +// Track is a representation of a physical track. +type Track struct { + width phys.Meters // total width, including border lanes + maxCofs phys.Meters // maximum ABS(Cofs) a vehicle can have + pieces []RoadPiece // in trackwise driving order; finish line = start of pieces[0] + entryPoses []phys.Pose // in trackwise driving order + entryDofs []phys.Meters // at piece entry: distance offset from finish line, along road center + minCorner phys.Point // minimum corner of the track (ie bottom-left) + maxCorner phys.Point // maximum corner of the track (ie upper-right) +} + +// NewTrack creates a track with a fixed width and a set of consecutive road +// pieces. +func NewTrack(width phys.Meters, maxCofs phys.Meters, pieces []RoadPiece) (*Track, error) { + // Sanity checking for input args + numRp := len(pieces) + if numRp < 4 { + return nil, fmt.Errorf("Track with %v road pieces is too small", numRp) + } + if width <= 0 { + return nil, fmt.Errorf("Invalid track width: %v", width) + } + if maxCofs == 0 { + maxCofs = width / 2 + } + if maxCofs < 0 { + return nil, fmt.Errorf("maxCofs=%v invalid; must be > 0", maxCofs) + } + + // compute per-piece info needed by the representation + t := Track{ + width: width, + maxCofs: maxCofs, + pieces: pieces, + entryPoses: make([]phys.Pose, numRp+1, numRp+1), + entryDofs: make([]phys.Meters, numRp+1, numRp+1), + } + + // the finish line is ALWAYS at the origin, facing right + t.entryPoses[0] = phys.Pose{Point: phys.Point{X: 0, Y: 0}, Theta: 0} + t.entryDofs[0] = 0 + for i, rp := range pieces { + t.entryDofs[i+1] = t.entryDofs[i] + rp.Len(0) + t.entryPoses[i+1] = t.entryPoses[i].AdvancePose(rp.DeltaPose()) + } + + // Determine the corners of the "world", by examining track edges at road + // piece boundaries. + for i, _ := range pieces { + for j := 0; j < 2; j++ { + deltaPose := phys.Pose{Point: phys.Point{X: 0, Y: (-width / 2) + phys.Meters(j)*width}, Theta: 0} + edge := t.entryPoses[i].AdvancePose(deltaPose) + if edge.X < t.minCorner.X { + t.minCorner.X = edge.X + } + if edge.Y < t.minCorner.Y { + t.minCorner.Y = edge.Y + } + if edge.X > t.maxCorner.X { + t.maxCorner.X = edge.X + } + if edge.Y > t.maxCorner.Y { + t.maxCorner.Y = edge.Y + } + } + } + + // The pose after doing a lap along road center should match starting pose + if phys.RadiansAreNear(t.entryPoses[0].Theta, t.entryPoses[numRp].Theta, TrackRadiansAreEqualTol) && + phys.MetersAreNear(t.entryPoses[0].X, t.entryPoses[numRp].X, TrackMetersAreEqualTol) && + phys.MetersAreNear(t.entryPoses[0].Y, t.entryPoses[numRp].Y, TrackMetersAreEqualTol) { + return &t, nil + } + + return &t, fmt.Errorf("Track is not a loop: beg pose = %s, end pose =%s", + t.entryPoses[0].String(), t.entryPoses[numRp].String()) +} + +// Width returns the width of the track. +func (t *Track) Width() phys.Meters { + return t.width +} + +// MaxCofs returns the maximum allowed horizontal offset. This may be greater or +// less than Track.Width()/2. +func (t *Track) MaxCofs() phys.Meters { + return t.maxCofs +} + +// NumRp returns the number of road pieces in the track. +func (t *Track) NumRp() int { + return len(t.pieces) +} + +// CenLen returns the track length along road center. +func (t *Track) CenLen() phys.Meters { + return t.entryDofs[len(t.pieces)] +} + +// Len computes the total driving path length of the track, along a given +// horizontal offset. +func (t *Track) Len(cofs phys.Meters) phys.Meters { + var len phys.Meters = 0 + for _, rp := range t.pieces { + len += rp.Len(cofs) + } + return len +} + +// MinCorner is the botom-left corner of the rectangle that completely encloses +// the track. +func (t *Track) MinCorner() phys.Point { + return t.minCorner +} + +// MaxCorner is the upper-right corner of the rectangle that completely encloses +// the track. +func (t *Track) MaxCorner() phys.Point { + return t.maxCorner +} + +// RpiAt returns the Road Piece Index corresponding to a distance offset. +func (t *Track) RpiAt(dofs phys.Meters) Rpi { + // XXX: Linear search + for i, _ := range t.entryDofs { + if t.entryDofs[i] > dofs { + return Rpi(i - 1) + } + } + panic(fmt.Sprintf("RpiAt(%v) with track len %v: Could not find road piece index", dofs, t.entryDofs[len(t.pieces)])) +} + +// assertValidDofs causes a panic of the Dofs value is not in an appropriate +// range for the track. +func (t *Track) assertValidDofs(dofs phys.Meters) { + if dofs < 0 { + panic(fmt.Sprintf("dofs=%v is invalid, must be >= 0", dofs)) + } + if dofs > t.CenLen() { + panic(fmt.Sprintf("dofs=%v is invalid, must be <= track.CenLen()=%v", dofs, t.CenLen())) + } +} + +// assertValidRpi causes a panic if the index is not valid for the track. +func (t *Track) assertValidRpi(i Rpi) { + if int(i) >= len(t.pieces) { + panic(fmt.Sprintf("rpi==%d is not valid for track with only %d pieces", i, len(t.pieces))) + } +} + +// Rp returns a Road Piece in the track, from it's Road Piece Index. +func (t *Track) Rp(i Rpi) RoadPiece { + t.assertValidRpi(i) + return t.pieces[i] +} + +// RpEntryDofs returns the Dofs value at the start of the road piece, in the +// trackwise driving direction. +func (t *Track) RpEntryDofs(i Rpi) phys.Meters { + t.assertValidRpi(i) + return t.entryDofs[i] +} + +// RpEntryPose returns the pose of the vehicle when it enters a road piece. This +// pose is for road center, trackwise driving direction. +func (t *Track) RpEntryPose(i Rpi) phys.Pose { + if int(i) == len(t.pieces) { + i = 0 + } + t.assertValidRpi(i) + return t.entryPoses[i] +} + +// RpCurveCenter returns the center point of the cirlce's radius of curvature. +// Straight pieces, which have no curvature, return the point of entry. +func (t *Track) RpCurveCenter(i Rpi) phys.Point { + t.assertValidRpi(i) + rp := t.pieces[i] + + // special case: straight + if rp.DAngle() == 0 { + return phys.Point{X: t.entryPoses[i].X, Y: t.entryPoses[i].Y} + } + + dy := rp.CurveRadius(0) + if rp.DAngle() < 0 { + dy = -dy + } + pose := t.entryPoses[i].AdvancePose(phys.Pose{Point: phys.Point{X: 0, Y: dy}, Theta: 0}) + return phys.Point{X: pose.X, Y: pose.Y} +} + +// RpiAndRpDofs maps a distance offset to a road piece and road-center distance +// into the road piece, for trackwise driving direction. +func (t *Track) RpiAndRpDofs(dofs phys.Meters) (Rpi, phys.Meters) { + t.assertValidDofs(dofs) + rpi := len(t.pieces) - 1 + for ; (rpi > 0) && (t.entryDofs[rpi] > dofs); rpi-- { + } + rpDofs := dofs - t.entryDofs[rpi] + if rpDofs > t.pieces[rpi].CenLen() { + // XXX: This can happen due to arithmetic precision errors + rpDofs = t.pieces[rpi].CenLen() + } + return Rpi(rpi), rpDofs +} + +// ToPose converts a Pose to a canonical Cartesian pose. +func (t *Track) ToPose(tp Pose) phys.Pose { + tp.Dofs = t.NormalizeDofs(tp.Dofs) + rpi, rpDofs := t.RpiAndRpDofs(tp.Dofs) + rp := t.Rp(rpi) + pose := t.RpEntryPose(rpi) + + // adjust Dofs + percent := float64(rpDofs / rp.CenLen()) + if math.Abs(percent) > 1.0e-6 { + rp2 := NewRoadPiece(phys.Meters(percent)*rp.CenLen(), phys.Radians(percent*float64(rp.DAngle()))) + pose = pose.AdvancePose(rp2.DeltaPose()) + } + + // adjust Cofs + pose = pose.AdvancePose(phys.Pose{Point: phys.Point{X: 0, Y: tp.Cofs}, Theta: 0}) + + // adjust Theta + pose.Theta = phys.NormalizeRadians(pose.Theta + tp.DAngle) + + return pose +} + +// NormalizeDofs adjusts a Dofs by increments of the track len, to make sure +// that (0 <= Dofs < track.CenLen()). +func (t *Track) NormalizeDofs(dofs phys.Meters) phys.Meters { + for ; dofs < 0; dofs += t.CenLen() { + } + for ; dofs >= t.CenLen(); dofs -= t.CenLen() { + } + return dofs +} + +// DofsDist returns the shortest distance between two track distance offsets, +// taking the looping nature of the track into account. Always positive. +func (t *Track) DofsDist(dofs1, dofs2 phys.Meters) phys.Meters { + dist := math.Abs(float64(dofs1) - float64(dofs2)) + if dist > (float64(t.CenLen()) / 2) { + dist = float64(t.CenLen()) - dist + } + return phys.Meters(dist) +} + +func (t *Track) isFacingTrackwise(dangle phys.Radians) bool { + if (dangle > math.Pi) || (dangle < -math.Pi) { + panic(fmt.Sprintf("isFacingTrackwise(dangle==%v) is invalid; must be in range [-pi,pi]", dangle)) + } + return (math.Abs(float64(dangle)) <= (math.Pi / 2)) +} + +// DriveDofsDist returns the Dofs distance needed to drive from Point A to dofs, +// in the track direction that Pose A is facing. For example, if dofs is a +// little behind Point A, then A would need to drive most of the track length to +// get to it. +func (t *Track) DriveDofsDist(a Pose, dofs phys.Meters) phys.Meters { + if t.isFacingTrackwise(a.DAngle) { + return t.NormalizeDofs(dofs - a.Dofs) + } + return t.NormalizeDofs(a.Dofs - dofs) +} + +// DriveDist calculates the driving distance needed to reach dofs starting from +// p, driving along center offset p.Cofs, in direction p.DAngle. Always +// positive. +func (t *Track) DriveDist(p Pose, dofs phys.Meters) phys.Meters { + dofs1 := p.Dofs + dofs2 := dofs + if !t.isFacingTrackwise(p.DAngle) { + dofs1, dofs2 = dofs2, dofs1 + } + rpi1, rpDofs1 := t.RpiAndRpDofs(dofs1) + rpi2, rpDofs2 := t.RpiAndRpDofs(dofs2) + + if (rpi1 == rpi2) && (dofs2 >= dofs1) { + dist := dofs2 - dofs1 + rp := t.Rp(rpi1) + if !rp.IsStraight() { + dist *= (rp.CurveRadius(p.Cofs) / rp.CurveRadius(0)) + } + return dist + } + + rpCount := int(rpi2 - rpi1) + if rpi2 <= rpi1 { + rpCount += t.NumRp() + } + dist := phys.Meters(0) + for i := 0; i < rpCount; i++ { + j := i + int(rpi1) + if j >= t.NumRp() { + j -= t.NumRp() + } + rpi := Rpi(j) + rp := t.Rp(rpi) + if rpi == rpi1 { + dist += rp.Len(p.Cofs) * ((rp.CenLen() - rpDofs1) / rp.CenLen()) + } else { + dist += rp.Len(p.Cofs) + } + } + rp2 := t.Rp(rpi2) + dist += rp2.Len(p.Cofs) * (rpDofs2 / rp2.CenLen()) + return dist +} + +// DriveDeltaDist calculates the driving distance needed to reach dofs starting +// from p, driving along center offset p.Cofs. However, if this distance is more +// than half of the track length at p.Cofs, it is considered BEHIND Pose p, and +// a negative distance is returned. +func (t *Track) DriveDeltaDist(p Pose, dofs phys.Meters) phys.Meters { + len := t.Len(p.Cofs) + dd := t.DriveDist(p, dofs) + if dd > (len / 2) { + dd -= len + } + return dd +} + +// DriveDeltaDofs returns the Dofs delta from Track Pose A to dofs, as if Pose +// were the origin. +// - Bounds: (-t.CenLen()/2) <= DeltaDriveDofs <= (t.CenLen()/2) +// - If dofs is is more than half of the track length ahead in the direction +// the Pose is facing, then it is considered BEHIND the Pose, so that +// DriveDeltaDofs<0. +func (t *Track) DriveDeltaDofs(a Pose, dofs phys.Meters) phys.Meters { + driveDofs := t.DriveDofsDist(a, dofs) + if driveDofs > (t.CenLen() / 2) { + return driveDofs - t.CenLen() + } + return driveDofs +} + +// DriveDeltaCofs returns the Cofs distance from a Track Pose A to cofs, as if +// Pose were the origin. For example, if Pose is facing counter-trackwise and +// Point is to its left, then DriveDeltaCofs>0. +func (t *Track) DriveDeltaCofs(a Pose, cofs phys.Meters) phys.Meters { + deltaCofs := cofs - a.Cofs + if t.isFacingTrackwise(a.DAngle) { + return deltaCofs + } + return -deltaCofs +} diff --git a/goverdrive/robo/track/track_test.go b/goverdrive/robo/track/track_test.go new file mode 100644 index 0000000..cac0fce --- /dev/null +++ b/goverdrive/robo/track/track_test.go @@ -0,0 +1,699 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package track + +import ( + "fmt" + "math" + "strings" + "testing" + + "github.com/anki/goverdrive/phys" +) + +////////////////////////////////////////////////////////////////////// + +const ( + defTrackWidth = 0.2 + nearMTolerance phys.Meters = 1.0e-6 // XXX + nearRTolerance phys.Radians = 1.0e-6 // XXX +) + +// testEqual reports a testing error if the two values are not equal +func testEqual(t *testing.T, tag string, exp interface{}, got interface{}) { + if exp != got { + t.Errorf("%s error: exp=%v, got=%v", tag, exp, got) + } +} + +// testMetersAreNear reports a testing error if the two Meters values are not +// near each other +func testMetersAreNear(t *testing.T, tag string, exp phys.Meters, got phys.Meters) { + if !phys.MetersAreNear(exp, got, nearMTolerance) { + t.Errorf("%s error: exp=%v, got=%v", tag, exp, got) + } +} + +// testRadiansAreNear reports a testing error if the two Radians values are not +// near each other +func testRadiansAreNear(t *testing.T, tag string, exp phys.Radians, got phys.Radians) { + if !phys.RadiansAreNear(exp, got, nearRTolerance) { + t.Errorf("%s error: exp=%v, got=%v", tag, exp, got) + } +} + +////////////////////////////////////////////////////////////////////// + +type testDofsVec struct { + dofs phys.Meters + expRpi Rpi + expRpDofs phys.Meters +} + +type testPoseVec struct { + dofs phys.Meters + expPose phys.Pose +} + +// TestLeftQuadraTracks checks basic track properties like width, length, entry +// poses, and road piece indexing. This is a convenient track for testing, since +// it has easily-computable geometry. +func TestLeftQuadraTracks(t *testing.T) { + for n := 1; n <= 5; n++ { // number of consecutive straights + topo := strings.Repeat("S", n) + "L" + + /******/ strings.Repeat("S", n) + "L" + + /******/ strings.Repeat("S", n) + "L" + + /******/ strings.Repeat("S", n) + "L" + width := phys.Meters(float32(n) * 0.1) + numRp := len(topo) + 1 + track, err := NewModularTrack(width, width/2, topo) + if err != nil { + t.Fatalf(err.Error()) + } + + // Verify basic track properties + testEqual(t, topo+" track.Width()", width, track.Width()) + testEqual(t, topo+" track.NumRp()", numRp, track.NumRp()) + + // Get one of the 90-degree curves in the track + curveRpi := Rpi(n) // first curve is after n straights + track.assertValidRpi(curveRpi) + curveRp := track.Rp(curveRpi) + testEqual(t, topo+" curveRp.IsStraight()", false, curveRp.IsStraight()) + + // Verify overall track length at different center offsets + for h := 0; h < 3; h++ { + cofs := phys.Meters(-0.3 + (float64(h) * 0.3)) + radius := curveRp.CurveRadius(cofs) + curveLen := phys.Meters(math.Pi/2) * radius + expLen := (phys.Meters(4*n) * TrackLenModStraight) + (phys.Meters(4) * curveLen) + gotLen := track.Len(cofs) + testMetersAreNear(t, topo+fmt.Sprintf(" track.Len(%v)", cofs), expLen, gotLen) + if phys.MetersAreNear(cofs, 0, nearMTolerance) { + testMetersAreNear(t, topo+" track.CenLen()", expLen, gotLen) + } + } + + // Verify overall size of the "world" + centerRadius := curveRp.CurveRadius(0) // road center + outerRadius := curveRp.CurveRadius(-width / 2) + expWorldWidth := (phys.Meters(n) * TrackLenModStraight) + (phys.Meters(2) * outerRadius) + gotWorldWidth := track.MaxCorner().X - track.MinCorner().X + testMetersAreNear(t, topo+" WorldWidth", expWorldWidth, gotWorldWidth) + expWorldHeight := (phys.Meters(n) * TrackLenModStraight) + (phys.Meters(2) * outerRadius) + gotWorldHeight := track.MaxCorner().X - track.MinCorner().X + testMetersAreNear(t, topo+" WorldHeight", expWorldHeight, gotWorldHeight) + + // Verify RpiAt() and RpiAndRpDofs() in a few places + tlen := track.Len(0) + dofsTestTable := []testDofsVec{ + testDofsVec{dofs: TrackLenModStartShort / 2.0 /********/, expRpi: Rpi(0) /*****/, expRpDofs: TrackLenModStartShort / 2.0}, + testDofsVec{dofs: tlen - TrackLenModStartLong /********/, expRpi: Rpi(numRp - 1), expRpDofs: 0}, + testDofsVec{dofs: tlen - TrackLenModStartLong - 0.1 /**/, expRpi: Rpi(numRp - 2), expRpDofs: TrackLenModCurve - 0.1}, + } + for _, v := range dofsTestTable { + rpi, rpDofs := track.RpiAndRpDofs(v.dofs) + testEqual(t, topo+fmt.Sprintf(" RpiAt(%v)", v.dofs), Rpi(v.expRpi), track.RpiAt(v.dofs)) + testEqual(t, topo+fmt.Sprintf(" RpiAndRpDofs(%v).Rpi", v.dofs), v.expRpi, rpi) + testMetersAreNear(t, topo+fmt.Sprintf(" RpiAndRpDofs(%v).RpDofs", v.dofs), v.expRpDofs, rpDofs) + } + + // Verify RpEntryPose() in a few places + for i := 1; i < (n + 1); i++ { + // First set of straights extends in +X direction + rpi := Rpi(i) + expEntryPose := phys.Pose{Point: phys.Point{ + X: TrackLenModStartShort + (phys.Meters(i-1) * TrackLenModStraight), + Y: 0}, + Theta: 0} + gotEntryPose := track.RpEntryPose(rpi) + testMetersAreNear(t, topo+fmt.Sprintf(" RpEntryPose(%v).X", rpi), expEntryPose.X, gotEntryPose.X) + testMetersAreNear(t, topo+fmt.Sprintf(" RpEntryPose(%v).Y", rpi), expEntryPose.Y, gotEntryPose.Y) + testRadiansAreNear(t, topo+fmt.Sprintf(" RpEntryPose(%v).Theta", rpi), expEntryPose.Theta, gotEntryPose.Theta) + + // Second set of straights goes in +Y direction + rpi = Rpi(n + i) + expEntryPose = phys.Pose{Point: phys.Point{ + X: centerRadius + (phys.Meters(n-1) * TrackLenModStraight) + TrackLenModStartShort, + Y: centerRadius + (phys.Meters(i-1) * TrackLenModStraight)}, + Theta: phys.Radians(math.Pi / 2)} + gotEntryPose = track.RpEntryPose(rpi) + testMetersAreNear(t, topo+fmt.Sprintf(" RpEntryPose(%v).X", rpi), expEntryPose.X, gotEntryPose.X) + testMetersAreNear(t, topo+fmt.Sprintf(" RpEntryPose(%v).Y", rpi), expEntryPose.Y, gotEntryPose.Y) + testRadiansAreNear(t, topo+fmt.Sprintf(" RpEntryPose(%v).Theta", rpi), expEntryPose.Theta, gotEntryPose.Theta) + } + + // Verify that Dofs <-> Pose is consistent at entry point of each road piece + for i := 0; i < track.NumRp(); i++ { + dofs := track.RpEntryDofs(Rpi(i)) + rpi, rpDofs := track.RpiAndRpDofs(dofs) + testEqual(t, topo+" rpi", Rpi(i), rpi) + testMetersAreNear(t, topo+" rpDofs", 0, rpDofs) + gotPose := track.ToPose(Pose{Point: Point{Dofs: dofs, Cofs: 0}, DAngle: 0}) + testMetersAreNear(t, topo+fmt.Sprintf(" ToPose(%v, 0).X", dofs), track.RpEntryPose(rpi).X, gotPose.X) + testMetersAreNear(t, topo+fmt.Sprintf(" ToPose(%v, 0).Y", dofs), track.RpEntryPose(rpi).Y, gotPose.Y) + testRadiansAreNear(t, topo+fmt.Sprintf(" ToPose(%v, 0).Theta", dofs), track.RpEntryPose(rpi).Theta, gotPose.Theta) + } + + // Verify ToPose() in a few more places + poseTestTable := []testPoseVec{ + testPoseVec{ + dofs: 0.1, + expPose: phys.Pose{Point: phys.Point{X: 0.1, Y: 0}, Theta: 0}}, + testPoseVec{ + dofs: 0.1 + TrackLenModStartShort + (phys.Meters(n-1) * TrackLenModStraight) + TrackLenModCurve, + expPose: phys.Pose{Point: phys.Point{X: phys.Meters(n-1)*TrackLenModStraight + TrackLenModStartShort + centerRadius, Y: 0.1 + centerRadius}, Theta: phys.Radians(math.Pi / 2)}}, + testPoseVec{ + dofs: 0.1 + TrackLenModStartShort + (phys.Meters(n-1) * TrackLenModStraight) + (phys.Meters(2*n) * TrackLenModStraight) + phys.Meters(3)*TrackLenModCurve, + expPose: phys.Pose{Point: phys.Point{X: -TrackLenModStartLong - centerRadius, Y: (phys.Meters(n) * TrackLenModStraight) + centerRadius - 0.1}, Theta: phys.Radians(-math.Pi / 2)}}, + } + for _, v := range poseTestTable { + pose := track.ToPose(Pose{Point: Point{Dofs: v.dofs, Cofs: 0}, DAngle: 0}) + testMetersAreNear(t, topo+fmt.Sprintf(" ToPose(%v, 0).X", v.dofs), v.expPose.X, pose.X) + testMetersAreNear(t, topo+fmt.Sprintf(" ToPose(%v, 0).Y", v.dofs), v.expPose.Y, pose.Y) + testRadiansAreNear(t, topo+fmt.Sprintf(" ToPose(%v, 0).Theta", v.dofs), v.expPose.Theta, pose.Theta) + } + + // Verify the center point of all four curves on the track + curveCenterDist := phys.Meters(n) * TrackLenModStraight + curveCenters := make([]phys.Point, 4, 4) // quadra corners, in driving order + curveCenters[3] = phys.Point{ // lower-left + X: -TrackLenModStartLong, + Y: centerRadius} + curveCenters[0] = phys.Point{ // lower-right + X: curveCenters[3].X + curveCenterDist, + Y: curveCenters[3].Y} + curveCenters[1] = phys.Point{ // upper-right + X: curveCenters[3].X + curveCenterDist, + Y: curveCenters[3].Y + curveCenterDist} + curveCenters[2] = phys.Point{ // upper-left + X: curveCenters[3].X, + Y: curveCenters[3].Y + curveCenterDist} + for i := 0; i < 4; i++ { + rpi := Rpi(n + i*(n+1)) // curve[i] + testMetersAreNear(t, topo+fmt.Sprintf(" RpCurveCenter(%v).X", rpi), curveCenters[i].X, track.RpCurveCenter(rpi).X) + testMetersAreNear(t, topo+fmt.Sprintf(" RpCurveCenter(%v).Y", rpi), curveCenters[i].Y, track.RpCurveCenter(rpi).Y) + } + } +} + +////////////////////////////////////////////////////////////////////// + +type testIsFacingTWVec struct { + dangle phys.Radians + exp bool +} + +type testDriveDofsDistVec struct { + frDofs phys.Meters + frDangle phys.Radians + toDofs phys.Meters + exp phys.Meters +} + +// TestTrackCoord tests miscellaneous functions related to track coordinates, +// poses, distance measurements, etc. +func TestTrackCoord(t *testing.T) { + trk, err := NewModularTrack(defTrackWidth, defTrackWidth/2, "SLLSSLLS") // Left Capsule + if err != nil { + t.Fatalf(err.Error()) + } + trkLen := trk.CenLen() + + fmt.Println("Check Track.NormalizeDofs()..") + for i := -3; i < 3; i++ { + for _, expNDofs := range []phys.Meters{0.1, 0.5, 1.0} { + dofs := expNDofs + (phys.Meters(i) * trkLen) + gotNDofs := trk.NormalizeDofs(dofs) + testMetersAreNear(t, fmt.Sprintf("NormalizeDofs(%v)", dofs), expNDofs, gotNDofs) + } + } + + fmt.Println("Check Track.isFacingTrackwise()..") + // Track.isFacingTrackwise() requires input angle to be in range [-pi,pi] + isFacingTWTestTable := []testIsFacingTWVec{ + testIsFacingTWVec{dangle: +0.0 * math.Pi, exp: true}, + testIsFacingTWVec{dangle: +0.1 * math.Pi, exp: true}, + testIsFacingTWVec{dangle: +0.4 * math.Pi, exp: true}, + testIsFacingTWVec{dangle: -0.1 * math.Pi, exp: true}, + testIsFacingTWVec{dangle: -0.4 * math.Pi, exp: true}, + + testIsFacingTWVec{dangle: +1.0 * math.Pi, exp: false}, + testIsFacingTWVec{dangle: +0.9 * math.Pi, exp: false}, + testIsFacingTWVec{dangle: +0.6 * math.Pi, exp: false}, + + testIsFacingTWVec{dangle: -1.0 * math.Pi, exp: false}, + testIsFacingTWVec{dangle: -0.9 * math.Pi, exp: false}, + testIsFacingTWVec{dangle: -0.6 * math.Pi, exp: false}, + } + for i, v := range isFacingTWTestTable { + label := fmt.Sprintf("Vec %d: isFacingTrackwise(%v)", i, v.dangle) + got := trk.isFacingTrackwise(v.dangle) + testEqual(t, label, v.exp, got) + } + + fmt.Println("Check Track.DriveDofsDist()..") + driveDofsDistTestTable := []testDriveDofsDistVec{ + testDriveDofsDistVec{frDofs: 0.1, frDangle: +0.0 * math.Pi, toDofs: 0.3, exp: 0.2}, + testDriveDofsDistVec{frDofs: 0.1, frDangle: +1.0 * math.Pi, toDofs: 0.3, exp: trkLen - 0.2}, + testDriveDofsDistVec{frDofs: 0.1, frDangle: -1.0 * math.Pi, toDofs: 0.3, exp: trkLen - 0.2}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: +0.0 * math.Pi, toDofs: trkLen - 0.400, exp: 0.0}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: +1.0 * math.Pi, toDofs: trkLen - 0.400, exp: 0.0}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: -1.0 * math.Pi, toDofs: trkLen - 0.400, exp: 0.0}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: +0.0 * math.Pi, toDofs: trkLen - 0.399, exp: 0.001}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: +1.0 * math.Pi, toDofs: trkLen - 0.399, exp: trkLen - 0.001}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: -1.0 * math.Pi, toDofs: trkLen - 0.399, exp: trkLen - 0.001}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: +0.0 * math.Pi, toDofs: trkLen - 0.401, exp: trkLen - 0.001}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: +1.0 * math.Pi, toDofs: trkLen - 0.401, exp: 0.001}, + testDriveDofsDistVec{frDofs: trkLen - 0.4, frDangle: -1.0 * math.Pi, toDofs: trkLen - 0.401, exp: 0.001}, + } + for i, v := range driveDofsDistTestTable { + pose := Pose{Point: Point{Dofs: v.frDofs, Cofs: 0}, DAngle: v.frDangle} + label := fmt.Sprintf("Vec %d: DriveDofsDist(%v, %v)", i, pose, v.toDofs) + got := trk.DriveDofsDist(pose, v.toDofs) + testMetersAreNear(t, label, v.exp, got) + } + +} + +////////////////////////////////////////////////////////////////////// + +type testDofsDistVec struct { + a phys.Meters + b phys.Meters + exp phys.Meters +} + +// TestDofsDist checks Track.DofsDist() +func TestDofDist(t *testing.T) { + trk, err := NewModularTrack(defTrackWidth, defTrackWidth/2, "SLLSSLLS") // Left Capsule + if err != nil { + t.Fatalf(err.Error()) + } + trkLen := trk.CenLen() + + dofsDistTestTable := []testDofsDistVec{ + testDofsDistVec{a: 0.0, b: 0.0, exp: 0.0}, + testDofsDistVec{a: 0.1, b: 0.2, exp: 0.1}, + testDofsDistVec{a: 0.2, b: 0.1, exp: 0.1}, + testDofsDistVec{a: 0.1, b: 1.0, exp: 0.9}, + testDofsDistVec{a: 1.0, b: 0.1, exp: 0.9}, + + testDofsDistVec{a: trkLen - 0.1, b: trkLen - 0.4, exp: 0.3}, + testDofsDistVec{a: trkLen - 0.4, b: trkLen - 0.1, exp: 0.3}, + testDofsDistVec{a: trkLen - 0.2, b: trkLen - 0.6, exp: 0.4}, + testDofsDistVec{a: trkLen - 0.6, b: trkLen - 0.2, exp: 0.4}, + + testDofsDistVec{a: trkLen - 0.1, b: 0.1 /******/, exp: 0.2}, + testDofsDistVec{a: 0.1 /******/, b: trkLen - 0.1, exp: 0.2}, + testDofsDistVec{a: trkLen - 0.5, b: 0.1 /******/, exp: 0.6}, + testDofsDistVec{a: 0.1 /******/, b: trkLen - 0.5, exp: 0.6}, + } + for _, v := range dofsDistTestTable { + label := fmt.Sprintf("Track.DofsDist(%v, %v)", v.a, v.b) + got := trk.DofsDist(v.a, v.b) + testMetersAreNear(t, label, v.exp, got) + } +} + +////////////////////////////////////////////////////////////////////// + +type testDriveDeltaDofsVec struct { + frDofs phys.Meters + frDangle phys.Radians + toDofs phys.Meters + exp phys.Meters +} + +type testDriveDeltaCofsVec struct { + frCofs phys.Meters + frDangle phys.Radians + toCofs phys.Meters + exp phys.Meters +} + +// TestDriveDelta tests Track.DriveDeltaXyz() functions +func TestDriveDelta(t *testing.T) { + for _, topo := range []string{"SLLSSLLS", "SRSLLLSSRR"} { // Left Capsule, Right Loopback + trk, err := NewModularTrack(defTrackWidth, defTrackWidth/2, topo) + if err != nil { + t.Fatalf(err.Error()) + } + trkLen := trk.CenLen() + halfTrkLen := trkLen / 2 + + dofsTestTable := []testDriveDeltaDofsVec{ + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +0.0 * math.Pi, toDofs: 0.0, exp: 0}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +1.0 * math.Pi, toDofs: 0.0, exp: 0}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: -1.0 * math.Pi, toDofs: 0.0, exp: 0}, + testDriveDeltaDofsVec{frDofs: 1.0, frDangle: +0.0 * math.Pi, toDofs: 1.0, exp: 0}, + testDriveDeltaDofsVec{frDofs: 1.0, frDangle: +1.0 * math.Pi, toDofs: 1.0, exp: 0}, + testDriveDeltaDofsVec{frDofs: 1.0, frDangle: -1.0 * math.Pi, toDofs: 1.0, exp: 0}, + + testDriveDeltaDofsVec{frDofs: 1.0, frDangle: +0.0 * math.Pi, toDofs: 1.2, exp: +0.2}, + testDriveDeltaDofsVec{frDofs: 1.2, frDangle: +0.0 * math.Pi, toDofs: 1.0, exp: -0.2}, + testDriveDeltaDofsVec{frDofs: 1.0, frDangle: +1.0 * math.Pi, toDofs: 1.2, exp: -0.2}, + testDriveDeltaDofsVec{frDofs: 1.2, frDangle: +1.0 * math.Pi, toDofs: 1.0, exp: +0.2}, + + testDriveDeltaDofsVec{frDofs: trkLen - 0.3, frDangle: +0.0 * math.Pi, toDofs: 0.2 /******/, exp: +0.5}, + testDriveDeltaDofsVec{frDofs: trkLen - 0.3, frDangle: -1.0 * math.Pi, toDofs: 0.2 /******/, exp: -0.5}, + testDriveDeltaDofsVec{frDofs: trkLen - 0.3, frDangle: +0.0 * math.Pi, toDofs: trkLen - 0.6, exp: -0.3}, + testDriveDeltaDofsVec{frDofs: trkLen - 0.3, frDangle: +1.0 * math.Pi, toDofs: trkLen - 0.6, exp: +0.3}, + + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +0.0 * math.Pi, toDofs: halfTrkLen, exp: halfTrkLen}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +1.0 * math.Pi, toDofs: halfTrkLen, exp: halfTrkLen}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: -1.0 * math.Pi, toDofs: halfTrkLen, exp: halfTrkLen}, + + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +0.0 * math.Pi, toDofs: halfTrkLen - 0.01, exp: +(halfTrkLen - 0.01)}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +1.0 * math.Pi, toDofs: halfTrkLen - 0.01, exp: -(halfTrkLen - 0.01)}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: +0.0 * math.Pi, toDofs: halfTrkLen + 0.01, exp: -(halfTrkLen - 0.01)}, + testDriveDeltaDofsVec{frDofs: 0.0, frDangle: -1.0 * math.Pi, toDofs: halfTrkLen + 0.01, exp: +(halfTrkLen - 0.01)}, + } + for i, v := range dofsTestTable { + pose := Pose{Point: Point{Dofs: v.frDofs, Cofs: 0.1}, DAngle: v.frDangle} + label := fmt.Sprintf("Vec %d: Track.DriveDeltaDofs(%v, %v)", i, pose, v.toDofs) + got := trk.DriveDeltaDofs(pose, v.toDofs) + testMetersAreNear(t, label, v.exp, got) + } + + cofsTestTable := []testDriveDeltaCofsVec{ + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +0.0 * math.Pi, toCofs: 0.0, exp: 0}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +1.0 * math.Pi, toCofs: 0.0, exp: 0}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: -1.0 * math.Pi, toCofs: 0.0, exp: 0}, + + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +0.0 * math.Pi, toCofs: +0.1, exp: +0.1}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +0.0 * math.Pi, toCofs: +0.3, exp: +0.3}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +0.0 * math.Pi, toCofs: -0.1, exp: -0.1}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +0.0 * math.Pi, toCofs: -0.4, exp: -0.4}, + + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +1.0 * math.Pi, toCofs: +0.1, exp: -0.1}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +1.0 * math.Pi, toCofs: +0.3, exp: -0.3}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +1.0 * math.Pi, toCofs: -0.1, exp: +0.1}, + testDriveDeltaCofsVec{frCofs: 0.0, frDangle: +1.0 * math.Pi, toCofs: -0.4, exp: +0.4}, + } + for i, v := range cofsTestTable { + pose := Pose{Point: Point{Dofs: 0, Cofs: v.frCofs}, DAngle: v.frDangle} + point := Point{Dofs: 0.01, Cofs: v.toCofs} + label := fmt.Sprintf("Vec %d: Track.DriveDeltaCofs(%v, %v)", i, pose, point) + got := trk.DriveDeltaCofs(pose, point.Cofs) + testMetersAreNear(t, label, v.exp, got) + } + } +} + +////////////////////////////////////////////////////////////////////// + +type testDriveDistStrtVec struct { + dofs1 phys.Meters + dangle1 phys.Radians + dofs2 phys.Meters + exp phys.Meters +} + +func TestDriveDistStraights(t *testing.T) { + // XXX: Use non-looping track, for easier test cases + pieces := []RoadPiece{ + *NewRoadPiece(1.0, 0.0), + *NewRoadPiece(1.0, 0.0), + *NewRoadPiece(1.0, 0.0), + *NewRoadPiece(1.0, 0.0), + } + trk, _ := NewTrack(defTrackWidth, defTrackWidth/2, pieces) + + testTable := []testDriveDistStrtVec{ + // Edge case: zero distance + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 0.0, exp: 0.0}, + testDriveDistStrtVec{dofs1: 0.0, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 0.0}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 0.0}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 1.0 * math.Pi, dofs2: 1.0, exp: 0.0}, + + // Within one road piece + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 0.1, exp: 0.1}, + testDriveDistStrtVec{dofs1: 0.2, dangle1: 0.0 * math.Pi, dofs2: 0.4, exp: 0.2}, + testDriveDistStrtVec{dofs1: 0.3, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 0.7}, + testDriveDistStrtVec{dofs1: 0.6, dangle1: 1.0 * math.Pi, dofs2: 0.5, exp: 0.1}, + testDriveDistStrtVec{dofs1: 0.5, dangle1: 1.0 * math.Pi, dofs2: 0.3, exp: 0.2}, + testDriveDistStrtVec{dofs1: 0.4, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 0.4}, + + testDriveDistStrtVec{dofs1: 1.0, dangle1: 0.0 * math.Pi, dofs2: 1.1, exp: 0.1}, + testDriveDistStrtVec{dofs1: 1.2, dangle1: 0.0 * math.Pi, dofs2: 1.4, exp: 0.2}, + testDriveDistStrtVec{dofs1: 1.3, dangle1: 0.0 * math.Pi, dofs2: 2.0, exp: 0.7}, + testDriveDistStrtVec{dofs1: 1.6, dangle1: 1.0 * math.Pi, dofs2: 1.5, exp: 0.1}, + testDriveDistStrtVec{dofs1: 1.5, dangle1: 1.0 * math.Pi, dofs2: 1.3, exp: 0.2}, + testDriveDistStrtVec{dofs1: 1.4, dangle1: 1.0 * math.Pi, dofs2: 1.0, exp: 0.4}, + + testDriveDistStrtVec{dofs1: 2.0, dangle1: 0.0 * math.Pi, dofs2: 2.1, exp: 0.1}, + testDriveDistStrtVec{dofs1: 2.2, dangle1: 0.0 * math.Pi, dofs2: 2.4, exp: 0.2}, + testDriveDistStrtVec{dofs1: 2.3, dangle1: 0.0 * math.Pi, dofs2: 3.0, exp: 0.7}, + testDriveDistStrtVec{dofs1: 2.6, dangle1: 1.0 * math.Pi, dofs2: 2.5, exp: 0.1}, + testDriveDistStrtVec{dofs1: 2.5, dangle1: 1.0 * math.Pi, dofs2: 2.3, exp: 0.2}, + testDriveDistStrtVec{dofs1: 2.4, dangle1: 1.0 * math.Pi, dofs2: 2.0, exp: 0.4}, + + testDriveDistStrtVec{dofs1: 3.0, dangle1: 0.0 * math.Pi, dofs2: 3.1, exp: 0.1}, + testDriveDistStrtVec{dofs1: 3.2, dangle1: 0.0 * math.Pi, dofs2: 3.4, exp: 0.2}, + testDriveDistStrtVec{dofs1: 3.3, dangle1: 0.0 * math.Pi, dofs2: 0.0, exp: 0.7}, + testDriveDistStrtVec{dofs1: 3.6, dangle1: 1.0 * math.Pi, dofs2: 3.5, exp: 0.1}, + testDriveDistStrtVec{dofs1: 3.5, dangle1: 1.0 * math.Pi, dofs2: 3.3, exp: 0.2}, + testDriveDistStrtVec{dofs1: 3.4, dangle1: 1.0 * math.Pi, dofs2: 3.0, exp: 0.4}, + + // Across two road pieces + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 2.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 1.9, exp: 1.9}, + testDriveDistStrtVec{dofs1: 0.2, dangle1: 0.0 * math.Pi, dofs2: 2.0, exp: 1.8}, + testDriveDistStrtVec{dofs1: 2.0, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 1.9, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 1.9}, + testDriveDistStrtVec{dofs1: 1.8, dangle1: 1.0 * math.Pi, dofs2: 0.3, exp: 1.5}, + + testDriveDistStrtVec{dofs1: 1.0, dangle1: 0.0 * math.Pi, dofs2: 3.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 0.0 * math.Pi, dofs2: 2.9, exp: 1.9}, + testDriveDistStrtVec{dofs1: 1.2, dangle1: 0.0 * math.Pi, dofs2: 3.0, exp: 1.8}, + testDriveDistStrtVec{dofs1: 3.0, dangle1: 1.0 * math.Pi, dofs2: 1.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 2.9, dangle1: 1.0 * math.Pi, dofs2: 1.0, exp: 1.9}, + testDriveDistStrtVec{dofs1: 2.8, dangle1: 1.0 * math.Pi, dofs2: 1.3, exp: 1.5}, + + testDriveDistStrtVec{dofs1: 2.0, dangle1: 0.0 * math.Pi, dofs2: 0.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 2.0, dangle1: 0.0 * math.Pi, dofs2: 3.9, exp: 1.9}, + testDriveDistStrtVec{dofs1: 2.2, dangle1: 0.0 * math.Pi, dofs2: 0.0, exp: 1.8}, + testDriveDistStrtVec{dofs1: 0.0, dangle1: 1.0 * math.Pi, dofs2: 2.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 3.9, dangle1: 1.0 * math.Pi, dofs2: 2.0, exp: 1.9}, + testDriveDistStrtVec{dofs1: 3.8, dangle1: 1.0 * math.Pi, dofs2: 2.3, exp: 1.5}, + + testDriveDistStrtVec{dofs1: 3.0, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 3.0, dangle1: 0.0 * math.Pi, dofs2: 0.9, exp: 1.9}, + testDriveDistStrtVec{dofs1: 3.2, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 1.8}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 1.0 * math.Pi, dofs2: 3.0, exp: 2.0}, + testDriveDistStrtVec{dofs1: 0.9, dangle1: 1.0 * math.Pi, dofs2: 3.0, exp: 1.9}, + testDriveDistStrtVec{dofs1: 0.8, dangle1: 1.0 * math.Pi, dofs2: 3.3, exp: 1.5}, + + // Across three road pieces + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 3.0, exp: 3.0}, + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 2.8, exp: 2.8}, + testDriveDistStrtVec{dofs1: 0.3, dangle1: 0.0 * math.Pi, dofs2: 3.0, exp: 2.7}, + testDriveDistStrtVec{dofs1: 0.4, dangle1: 0.0 * math.Pi, dofs2: 2.6, exp: 2.2}, + testDriveDistStrtVec{dofs1: 3.0, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 3.0}, + testDriveDistStrtVec{dofs1: 2.7, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 2.7}, + testDriveDistStrtVec{dofs1: 3.0, dangle1: 1.0 * math.Pi, dofs2: 0.4, exp: 2.6}, + testDriveDistStrtVec{dofs1: 2.6, dangle1: 1.0 * math.Pi, dofs2: 0.5, exp: 2.1}, + + testDriveDistStrtVec{dofs1: 2.0, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 3.0}, + testDriveDistStrtVec{dofs1: 2.0, dangle1: 0.0 * math.Pi, dofs2: 0.8, exp: 2.8}, + testDriveDistStrtVec{dofs1: 2.3, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 2.7}, + testDriveDistStrtVec{dofs1: 2.4, dangle1: 0.0 * math.Pi, dofs2: 0.6, exp: 2.2}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 1.0 * math.Pi, dofs2: 2.0, exp: 3.0}, + testDriveDistStrtVec{dofs1: 0.7, dangle1: 1.0 * math.Pi, dofs2: 2.0, exp: 2.7}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 1.0 * math.Pi, dofs2: 2.4, exp: 2.6}, + testDriveDistStrtVec{dofs1: 0.6, dangle1: 1.0 * math.Pi, dofs2: 2.5, exp: 2.1}, + + // Accross four road pieces + testDriveDistStrtVec{dofs1: 0.0, dangle1: 0.0 * math.Pi, dofs2: 3.9, exp: 3.9}, + testDriveDistStrtVec{dofs1: 0.2, dangle1: 0.0 * math.Pi, dofs2: 0.0, exp: 3.8}, + testDriveDistStrtVec{dofs1: 0.4, dangle1: 0.0 * math.Pi, dofs2: 0.1, exp: 3.7}, + testDriveDistStrtVec{dofs1: 3.9, dangle1: 1.0 * math.Pi, dofs2: 0.0, exp: 3.9}, + testDriveDistStrtVec{dofs1: 0.0, dangle1: 1.0 * math.Pi, dofs2: 0.2, exp: 3.8}, + testDriveDistStrtVec{dofs1: 0.1, dangle1: 1.0 * math.Pi, dofs2: 0.4, exp: 3.7}, + + testDriveDistStrtVec{dofs1: 1.0, dangle1: 0.0 * math.Pi, dofs2: 0.9, exp: 3.9}, + testDriveDistStrtVec{dofs1: 1.2, dangle1: 0.0 * math.Pi, dofs2: 1.0, exp: 3.8}, + testDriveDistStrtVec{dofs1: 1.4, dangle1: 0.0 * math.Pi, dofs2: 1.1, exp: 3.7}, + testDriveDistStrtVec{dofs1: 0.9, dangle1: 1.0 * math.Pi, dofs2: 1.0, exp: 3.9}, + testDriveDistStrtVec{dofs1: 1.0, dangle1: 1.0 * math.Pi, dofs2: 1.2, exp: 3.8}, + testDriveDistStrtVec{dofs1: 1.1, dangle1: 1.0 * math.Pi, dofs2: 1.4, exp: 3.7}, + + testDriveDistStrtVec{dofs1: 2.0, dangle1: 0.0 * math.Pi, dofs2: 1.9, exp: 3.9}, + testDriveDistStrtVec{dofs1: 2.2, dangle1: 0.0 * math.Pi, dofs2: 2.0, exp: 3.8}, + testDriveDistStrtVec{dofs1: 2.4, dangle1: 0.0 * math.Pi, dofs2: 2.1, exp: 3.7}, + testDriveDistStrtVec{dofs1: 1.9, dangle1: 1.0 * math.Pi, dofs2: 2.0, exp: 3.9}, + testDriveDistStrtVec{dofs1: 2.0, dangle1: 1.0 * math.Pi, dofs2: 2.2, exp: 3.8}, + testDriveDistStrtVec{dofs1: 2.1, dangle1: 1.0 * math.Pi, dofs2: 2.4, exp: 3.7}, + + testDriveDistStrtVec{dofs1: 3.0, dangle1: 0.0 * math.Pi, dofs2: 2.9, exp: 3.9}, + testDriveDistStrtVec{dofs1: 3.2, dangle1: 0.0 * math.Pi, dofs2: 3.0, exp: 3.8}, + testDriveDistStrtVec{dofs1: 3.4, dangle1: 0.0 * math.Pi, dofs2: 3.1, exp: 3.7}, + testDriveDistStrtVec{dofs1: 2.9, dangle1: 1.0 * math.Pi, dofs2: 3.0, exp: 3.9}, + testDriveDistStrtVec{dofs1: 3.0, dangle1: 1.0 * math.Pi, dofs2: 3.2, exp: 3.8}, + testDriveDistStrtVec{dofs1: 3.1, dangle1: 1.0 * math.Pi, dofs2: 3.4, exp: 3.7}, + } + for i, v := range testTable { + pose := Pose{Point: Point{Dofs: v.dofs1, Cofs: 0}, DAngle: v.dangle1} + label := fmt.Sprintf("Vec %d: Track.DriveDist(%v, %v)", i, pose, v.dofs2) + got := trk.DriveDist(pose, v.dofs2) + testMetersAreNear(t, label, v.exp, got) + } +} + +////////////////////////////////////////////////////////////////////// + +type testRegionOffset struct { + ofs phys.Meters + isInRegion bool +} + +// regions that DO NOT cross the finish line +func TestTrackRegionsDoNotCrossFinishLine(t *testing.T) { + topo := "SSRRSSRR" // Right Capsule + width := phys.Meters(0.3) + track, err := NewModularTrack(width, width/2, topo) + if err != nil { + t.Fatalf(err.Error()) + } + + for i := 0; i < 5; i++ { + dofs := phys.Meters(0.35 * float64(i)) + cofs := phys.Meters(0.5 - (0.11 * float64(i))) + len := phys.Meters(0.3 * float64(i+1)) + width := phys.Meters(0.1 * float64(i+1)) // NOTE: can be wider than the track + tr := NewRegion(track, Point{Dofs: dofs, Cofs: cofs}, len, width) + testEqual(t, "tr.Len()", len, tr.Len()) + testEqual(t, "tr.Width()", width, tr.Width()) + testEqual(t, "tr.C1().Dofs", dofs /********/, tr.C1().Dofs) + testEqual(t, "tr.C1().Cofs", cofs /********/, tr.C1().Cofs) + testEqual(t, "tr.C2().Dofs", dofs+len /****/, tr.C2().Dofs) + testEqual(t, "tr.C2().Cofs", cofs+width /**/, tr.C2().Cofs) + testEqual(t, "tr.CrossesFinishLine()", false, tr.CrossesFinishLine()) + + // Cherry-pick some offsets of interest that are inside and outside the + // region. Note that the edges of the regions are inclusive at the start, + // and exclusive at the end, for both distance and center dimensions. + dofsVals := []testRegionOffset{ + testRegionOffset{ofs: dofs - 0.1 + 0.0, isInRegion: false}, + testRegionOffset{ofs: dofs + 0.0 + 0.0, isInRegion: true}, + testRegionOffset{ofs: dofs + 0.1 + 0.0, isInRegion: true}, + testRegionOffset{ofs: dofs + len + 0.0, isInRegion: false}, + testRegionOffset{ofs: dofs + len + 0.1, isInRegion: false}, + } + cofsVals := []testRegionOffset{ + testRegionOffset{ofs: cofs - 0.100 + 0.0, isInRegion: false}, + testRegionOffset{ofs: cofs + 0.000 + 0.0, isInRegion: true}, + testRegionOffset{ofs: cofs + 0.050 + 0.0, isInRegion: true}, + testRegionOffset{ofs: cofs + width + 0.0, isInRegion: false}, + testRegionOffset{ofs: cofs + width + 0.1, isInRegion: false}, + } + for _, d := range dofsVals { + for _, c := range cofsVals { + tp := Point{Dofs: d.ofs, Cofs: c.ofs} + expContains := d.isInRegion && c.isInRegion + condStr := fmt.Sprintf("tr=%v ContainsPoint(%v)", tr, tp) + testEqual(t, condStr, expContains, tr.ContainsPoint(tp)) + } + } + } +} + +// regions that DO cross the finish line +func TestTrackRegionsDoCrossFinishLine(t *testing.T) { + topo := "SSRRSSRR" // Right Capsule + width := phys.Meters(0.3) + track, err := NewModularTrack(width, width/2, topo) + if err != nil { + t.Fatalf(err.Error()) + } + + for i := 0; i < 5; i++ { + dofs := track.CenLen() - phys.Meters(0.1*float64(i+1)) + cofs := phys.Meters(0.5 - (0.11 * float64(i))) + len := phys.Meters(0.3 * float64(i+1)) + width := phys.Meters(0.1 * float64(i+1)) // NOTE: can be wider than the track + tr := NewRegion(track, Point{Dofs: dofs, Cofs: cofs}, len, width) + testEqual(t, "tr.Len()", len, tr.Len()) + testEqual(t, "tr.Width()", width, tr.Width()) + testEqual(t, "tr.C1().Dofs", dofs /****************/, tr.C1().Dofs) + testEqual(t, "tr.C1().Cofs", cofs /****************/, tr.C1().Cofs) + testEqual(t, "tr.C2().Dofs", dofs+len-track.CenLen(), tr.C2().Dofs) + testEqual(t, "tr.C2().Cofs", cofs+width /**********/, tr.C2().Cofs) + testEqual(t, "tr.CrossesFinishLine()", true, tr.CrossesFinishLine()) + + // Cherry-pick some offsets of interest that are inside and outside the + // region. Note that the edges of the regions are inclusive at the start, + // and exclusive at the end, for both distance and center dimensions. + dofsVals := []testRegionOffset{ + testRegionOffset{ofs: dofs - 0.1 + 0.0, isInRegion: false}, + testRegionOffset{ofs: dofs + 0.0 + 0.0, isInRegion: true}, + testRegionOffset{ofs: dofs + 0.1 + 0.0, isInRegion: true}, + testRegionOffset{ofs: dofs + len + 0.0, isInRegion: false}, + testRegionOffset{ofs: dofs + len + 0.1, isInRegion: false}, + } + cofsVals := []testRegionOffset{ + testRegionOffset{ofs: cofs - 0.100 + 0.0, isInRegion: false}, + testRegionOffset{ofs: cofs + 0.000 + 0.0, isInRegion: true}, + testRegionOffset{ofs: cofs + 0.050 + 0.0, isInRegion: true}, + testRegionOffset{ofs: cofs + width + 0.0, isInRegion: false}, + testRegionOffset{ofs: cofs + width + 0.1, isInRegion: false}, + } + for _, d := range dofsVals { + for _, c := range cofsVals { + tp := Point{Dofs: d.ofs, Cofs: c.ofs} + for j := 0; j < 2; j++ { + if (j == 1) && (tp.Dofs >= track.CenLen()) { + tp.Dofs -= track.CenLen() // normalize + } + expContains := d.isInRegion && c.isInRegion + condStr := fmt.Sprintf("tr=%v ContainsPoint(%v)", tr, tp) + testEqual(t, condStr, expContains, tr.ContainsPoint(tp)) + } + } + } + } +} + +// test regions with (tr.Len >= track.CenLen()) +func TestTrackRegionsFullLength(t *testing.T) { + topo := "SSRRSSRR" // Right Capsule + width := phys.Meters(0.4) + track, err := NewModularTrack(width, width/2, topo) + if err != nil { + t.Fatalf(err.Error()) + } + + for i := 0; i < 5; i++ { + dofs := phys.Meters(0.2 * float64(i)) + cofs := phys.Meters(-0.1) + len := track.CenLen() + phys.Meters(0.1*float64(i)) + width := phys.Meters(0.2) + tr := NewRegion(track, Point{Dofs: dofs, Cofs: cofs}, len, width) + testEqual(t, "tr.Len()", len, tr.Len()) + testEqual(t, "tr.Width()", width, tr.Width()) + testEqual(t, "tr.C1().Dofs", dofs, tr.C1().Dofs) + testEqual(t, "tr.C1().Cofs", cofs, tr.C1().Cofs) + testEqual(t, "tr.CrossesFinishLine()", true, tr.CrossesFinishLine()) + + // Cherry-pick some offsets of interest that are inside and outside the + // region. Note that the edges of the regions are inclusive at the start, + // and exclusive at the end, for both distance and center dimensions. + cofsVals := []testRegionOffset{ + testRegionOffset{ofs: cofs - 0.100 + 0.0, isInRegion: false}, + testRegionOffset{ofs: cofs + 0.000 + 0.0, isInRegion: true}, + testRegionOffset{ofs: cofs + 0.050 + 0.0, isInRegion: true}, + testRegionOffset{ofs: cofs + width + 0.0, isInRegion: false}, + testRegionOffset{ofs: cofs + width + 0.1, isInRegion: false}, + } + for d := phys.Meters(0); d < (track.CenLen() + phys.Meters(0.4)); d += phys.Meters(0.1) { + for _, c := range cofsVals { + tp := Point{Dofs: d, Cofs: c.ofs} + expContains := c.isInRegion + condStr := fmt.Sprintf("tr=%v ContainsPoint(%v)", tr, tp) + testEqual(t, condStr, expContains, tr.ContainsPoint(tp)) + } + } + } +} diff --git a/goverdrive/robo/track/trackgen.go b/goverdrive/robo/track/trackgen.go new file mode 100644 index 0000000..b68f912 --- /dev/null +++ b/goverdrive/robo/track/trackgen.go @@ -0,0 +1,247 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com +// +// trackgen provides convenience functions to generate common tracks, such as +// the standard tracks in the OverDrive starter kit. + +package track + +import ( + "fmt" + "math" + "strconv" + "strings" + + "github.com/anki/goverdrive/phys" +) + +// NewModularTrack constructs a track using standard modular track pieces: +// S = straight +// R = right turn (90 degrees) +// L = left turn (90 degrees) + +// Examples: +// topo="SRRSSRRS" => Right Capsule +// topo="SLSRRRSSLL" => Left Loopback +// +// The first letter of the topo string must be `S`, and this will become the +// standard Short/Long start piece; the first track piece is Start Short and the +// last track piece is Start Long. +func NewModularTrack(width phys.Meters, maxCofs phys.Meters, topo string) (*Track, error) { + if topo[0] != 'S' { + return nil, fmt.Errorf("NewModularTrack topo string must start with 'S'. topo=%s", topo) + } + numRp := len(topo) + 1 // 1st straight is two road pieces + pieces := make([]RoadPiece, numRp, numRp) + + for i, tc := range topo { + if i == 0 { + pieces[0] = *NewRoadPiece(TrackLenModStartShort, 0) + continue + } + switch tc { + case 'S': + pieces[i] = *NewRoadPiece(TrackLenModStraight, 0) + case 'L': + pieces[i] = *NewRoadPiece(TrackLenModCurve, phys.Radians90DegreeTurnL) + case 'R': + pieces[i] = *NewRoadPiece(TrackLenModCurve, phys.Radians90DegreeTurnR) + default: + return nil, fmt.Errorf("Unsupported character in track topology string: %v", tc) + } + } + pieces[numRp-1] = *NewRoadPiece(TrackLenModStartLong, 0) + + return NewTrack(width, maxCofs, pieces) +} + +// kStarterKitTracks defines topology strings for starter kit tracks. Do not +// modify this variable! +var kStarterKitTracks = map[string]string{ + "cap": "SLLSLL", // XXX: "Cap" is a shorthand for "Microloop" + "lcap": "SLLSLL", + "rcap": "SRRSRR", + "microloop": "SLLSLL", + "lmicroloop": "SLLSLL", + "rmicroloop": "SRRSRR", + "capsule": "SLLSSLLS", + "lcapsule": "SLLSSLLS", + "rcapsule": "SRRSSRRS", + "quadra": "SLSLSLSL", + "lquadra": "SLSLSLSL", + "rquadra": "SRSRSRSR", + "point": "SLSLSLRLLS", + "lpoint": "SLSLSLRLLS", + "rpoint": "SRSRSRLRRS", + "wedge": "SLSLLRLL", + "lwedge": "SLSLLRLL", + "rwedge": "SRSRRLRR", + "hook": "SSLSLLRSLL", + "lhook": "SSLSLLRSLL", + "rhook": "SSRSRRLSRR", + "overpass": "SLLLSRRR", + "loverpass": "SLLLSRRR", + "roverpass": "SRRRSLLL", + "loopback": "SLSRRRSSLL", + "lloopback": "SLSRRRSSLL", + "rloopback": "SRSLLLSSRR", +} + +// StarterKitTrackNames returns a string with all of the supported starter kit +// track names. +func StarterKitTrackNames(seperator string) string { + s := "" + for name, _ := range kStarterKitTracks { + s += name + seperator + } + return s +} + +// NewStarterKitTrack constructs a variant of one of the OverDrive starter kit +// modular tracks. +// +// The track string has the form "Name_N", where: +// Name = {microloop, capsule, quadra, point, wedge, hook, overpass, loopback} +// Can add 'l' or 'r' prefix to any name to dictate the direction of +// the first curve. +// _N = (optional) replace each straight piece with N straight pieces +// This only creates a valid track for a subset of the tracks. +func NewStarterKitTrack(width phys.Meters, maxCofs phys.Meters, trackStr string) (*Track, error) { + trackStr = strings.ToLower(trackStr) + trackName := trackStr + paramIdx := strings.Index(trackStr, "_") + straightRep := 1 + if paramIdx != -1 { + trackName = trackStr[0:paramIdx] + val, err := strconv.Atoi(trackStr[paramIdx+1:]) + if err == nil { + straightRep = val + } + } + + topo, ok := kStarterKitTracks[trackName] + if !ok { + return nil, fmt.Errorf("trackName=%s is not recognized", trackName) + } + topo = strings.Replace(topo, "S", strings.Repeat("S", straightRep), -1) + + return NewModularTrack(width, maxCofs, topo) +} + +////////////////////////////////////////////////////////////////////// + +// kCustomTrackNames keeps the list of names, for documentation strings. +var kCustomTrackNames = map[string]bool{ + "miniocto": true, + "miniquadra": true, + "minicap": true, + "minirhom": true, + "minitrap": true, + "triangle": true, + "go": true, + "oval": true, +} + +// CustomTrackNames returns a string with all of the supported custom track +// names. +func CustomTrackNames(seperator string) string { + s := "" + for name, _ := range kCustomTrackNames { + s += name + seperator + } + return s +} + +// NewCustomTrack constructs a specific named custom track. +func NewCustomTrack(width phys.Meters, maxCofs phys.Meters, name string) (*Track, error) { + if !kCustomTrackNames[name] { + return nil, fmt.Errorf("Custom track name=%v is not recognized", name) + } + + // "mini" tracks are built with short straights and 45-degree turns + miniStraight := *NewRoadPiece(0.3, 0.0) + miniCurve45 := *NewRoadPiece(0.3, math.Pi/4) + + switch strings.ToLower(name) { + case "miniocto": // octogon + pieces := []RoadPiece{ + miniStraight, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniCurve45, + } + return NewTrack(width, maxCofs, pieces) + + case "miniquadra": + pieces := []RoadPiece{ + miniStraight, miniStraight, miniCurve45, miniCurve45, + miniStraight, miniStraight, miniCurve45, miniCurve45, + miniStraight, miniStraight, miniCurve45, miniCurve45, + miniStraight, miniStraight, miniCurve45, miniCurve45, + } + return NewTrack(width, maxCofs, pieces) + + case "minicap": // capsule + pieces := []RoadPiece{ + miniStraight, miniStraight, miniStraight, miniStraight, miniCurve45, miniCurve45, miniCurve45, miniCurve45, + miniStraight, miniStraight, miniStraight, miniStraight, miniCurve45, miniCurve45, miniCurve45, miniCurve45, + } + return NewTrack(width, maxCofs, pieces) + + case "minirhom": // rhombus + pieces := []RoadPiece{ + miniStraight, miniStraight, miniCurve45, miniCurve45, miniCurve45, + miniStraight, miniStraight, miniCurve45, + miniStraight, miniStraight, miniCurve45, miniCurve45, miniCurve45, + miniStraight, miniStraight, miniCurve45, + } + return NewTrack(width, maxCofs, pieces) + + case "minitrap": // trapezoid + pieces := []RoadPiece{ + miniStraight, miniStraight, miniCurve45, miniCurve45, miniCurve45, + miniStraight, miniCurve45, + miniStraight, miniStraight, miniCurve45, miniCurve45, miniCurve45, + miniStraight, miniCurve45, + } + return NewTrack(width, maxCofs, pieces) + + case "triangle": + pieces := []RoadPiece{ + *NewRoadPiece(1.0, 0.0), + *NewRoadPiece(0.3, 2*math.Pi/6), + *NewRoadPiece(0.3, 2*math.Pi/6), + *NewRoadPiece(1.0, 0.0), + *NewRoadPiece(0.3, 2*math.Pi/6), + *NewRoadPiece(0.3, 2*math.Pi/6), + *NewRoadPiece(1.0, 0.0), + *NewRoadPiece(0.3, 2*math.Pi/6), + *NewRoadPiece(0.3, 2*math.Pi/6), + } + return NewTrack(width, maxCofs, pieces) + + case "go": + return NewModularTrack(width, maxCofs, "SSSSLSLSLSRLSRSRRRLLSSSLSLSL") + + case "oval": + pieces := []RoadPiece{ + *NewRoadPiece(0.30, 0.0), + *NewRoadPiece(1.20, 0.0), + *NewRoadPiece(0.45, math.Pi/2.67), + *NewRoadPiece(0.45, math.Pi/2.67), + *NewRoadPiece(0.45, math.Pi/2.67), + *NewRoadPiece(1.25, -2*math.Pi/2.7/3), + *NewRoadPiece(0.45, math.Pi/2.67), + *NewRoadPiece(0.45, math.Pi/2.67), + *NewRoadPiece(0.45, math.Pi/2.67), + } + return NewTrack(width, maxCofs, pieces) + + default: + return nil, fmt.Errorf("Custom track name=%v is not recognized", name) + } +} diff --git a/goverdrive/robo/track/trackregion.go b/goverdrive/robo/track/trackregion.go new file mode 100644 index 0000000..43000ab --- /dev/null +++ b/goverdrive/robo/track/trackregion.go @@ -0,0 +1,114 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package track + +import ( + "fmt" + + "github.com/anki/goverdrive/phys" +) + +// Region defines a "rectangular" sub-region of the track. If the +// definition includes areas of curvature, the region bends to match the shape +// of the track. +// - The start corner's Dofs must satisfy (0 <= Dofs <= track.CenLen()) +// - The length of the region can be >track.CenLen() => covers whole track +// - Cofs can extend beyond the width of the track +// - Regions always extend in the forward driving direction from first corner, +// and always in the +Cofs direction +type Region struct { + c1 Point // start corner + len phys.Meters + width phys.Meters + track *Track +} + +func (tr *Region) String() string { + return fmt.Sprintf("Region{c1: %v, len: %v, width: %v}", tr.c1, tr.len, tr.width) +} + +// NewRegion creates a track region by specifying its start corner, length, +// and width. +func NewRegion(track *Track, c1 Point, len, width phys.Meters) *Region { + // check input values + if c1.Dofs < 0 { + panic(fmt.Sprintf("NewRegion: c1.Dofs=%v invalid; must be >= 0", c1.Dofs)) + } + if c1.Dofs >= track.CenLen() { + panic(fmt.Sprintf("NewRegion: c1.Dofs=%v invalid; must be < track.CenLen()=%v", c1.Dofs, track.CenLen())) + } + if width <= 0 { + panic(fmt.Sprintf("NewRegion: width=%v invalid; must be >0", width)) + } + if len <= 0 { + panic(fmt.Sprintf("NewRegion: len=%v invalid; must be >0", len)) + } + + return &Region{ + c1: c1, + len: len, + width: width, + track: track, + } +} + +// C1 returns the "start" corner of the track region. +func (tr *Region) C1() Point { + return tr.c1 +} + +// C2 returns the "end" corner of the track region. +func (tr *Region) C2() Point { + c2 := tr.c1 + c2.Cofs += tr.width + c2.Dofs = tr.track.NormalizeDofs(c2.Dofs + tr.len) + return c2 +} + +// Width returns the width of the track region. +func (tr *Region) Width() phys.Meters { + return tr.width +} + +// Len returns the length of the track region. +func (tr *Region) Len() phys.Meters { + return tr.len +} + +// CrossesFinishLine returns true if the track region crosses the finish line. +func (tr *Region) CrossesFinishLine() bool { + return (tr.c1.Dofs + tr.len) >= tr.track.CenLen() +} + +// ContainsPoint returns true if a track point is contained inside the track +// region. Note that corner C1 is included in the region, but C2 is not. In +// other words, the rectangular track region is [C1, C2). +func (tr *Region) ContainsPoint(p Point) bool { + // center offset + // vvv Inclusive vvvv Exclusive + if (p.Cofs < tr.c1.Cofs) || (p.Cofs >= (tr.c1.Cofs + tr.width)) { + return false + } + + // distance offset + p.Dofs = tr.track.NormalizeDofs(p.Dofs) + if tr.CrossesFinishLine() { + if p.Dofs >= tr.c1.Dofs { + return true + } + c2Dofs := tr.track.NormalizeDofs(tr.c1.Dofs + tr.len) + if p.Dofs < c2Dofs { + return true + } + return false + } else { + // region does not cross finish line + // vvv Inclusive vvvv Exclusive + if (p.Dofs < tr.c1.Dofs) || (p.Dofs >= (tr.c1.Dofs + tr.len)) { + return false + } + } + + return true +} diff --git a/goverdrive/robo/vehicle.go b/goverdrive/robo/vehicle.go new file mode 100644 index 0000000..e3e4dd7 --- /dev/null +++ b/goverdrive/robo/vehicle.go @@ -0,0 +1,296 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package robo + +import ( + "fmt" + "image/color" + "math" + + cn "golang.org/x/image/colornames" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo/light" + "github.com/anki/goverdrive/robo/track" +) + +const ( + // DefUturnRadius is the "typical" turn radius for a non-truck vehicle + DefUturnRadius phys.Meters = 0.05 +) + +////////////////////////////////////////////////////////////////////// +// Vehicle Types +////////////////////////////////////////////////////////////////////// + +// VehType is a two-letter abbreviation for a vehicle "type" (aka "model" in +// some contexts). Using two-letter names is simple, concise, and lines up +// nicely for table-driven code. +// +// Examples: +// "gs" = Groundshock +// "sk" = Skull +// "th" = Thermo +type VehType string // two letters, lowercase (eg "gs", "sk") + +// VehTypeInfo stores name, physical properties, etc for a vehicle. +type VehTypeInfo struct { + FullName string // eg "Groundshock" + Color color.Color + Width phys.Meters + Length phys.Meters + Mass phys.Grams +} + +// TODO: Better to put vehicle info into JSON file(s)? +var vehTypeInfoTable = map[VehType]VehTypeInfo{ + "gs": VehTypeInfo{FullName: "Groundshock" /**/, Color: cn.Royalblue /*******/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "sk": VehTypeInfo{FullName: "Skull" /********/, Color: cn.Darkslategray /***/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "nk": VehTypeInfo{FullName: "Nuke" /*********/, Color: cn.Limegreen /*******/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "th": VehTypeInfo{FullName: "Thermo" /*******/, Color: cn.Orangered /*******/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "gu": VehTypeInfo{FullName: "Guardian" /*****/, Color: cn.Skyblue /*********/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "bb": VehTypeInfo{FullName: "BigBang" /******/, Color: cn.Seagreen /********/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "fw": VehTypeInfo{FullName: "Freewheel" /****/, Color: cn.Lime /************/, Width: 0.044, Length: 0.24, Mass: 40.0}, + "xr": VehTypeInfo{FullName: "X52" /**********/, Color: cn.Red /*************/, Width: 0.044, Length: 0.24, Mass: 40.0}, + "xi": VehTypeInfo{FullName: "X52Ice" /*******/, Color: cn.White /***********/, Width: 0.044, Length: 0.24, Mass: 40.0}, + "dy": VehTypeInfo{FullName: "Dynamo" /*******/, Color: cn.Darkgray /********/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "mm": VehTypeInfo{FullName: "Mammoth" /******/, Color: cn.Lightsteelblue /**/, Width: 0.044, Length: 0.08, Mass: 40.0}, + "np": VehTypeInfo{FullName: "NukePhantom" /**/, Color: cn.Ghostwhite /******/, Width: 0.044, Length: 0.08, Mass: 40.0}, +} + +////////////////////////////////////////////////////////////////////// +// Vehicle +////////////////////////////////////////////////////////////////////// + +// Vehicle models a vehicle's robotics state and qualities, for simulation. It +// does NOT capture any game-specific state or behavior, such as weapons +// game-imposed min/max speeds, lap counts, etc. +// +// Abbreviations: +// cmd = commanded (eventual) +// des = desired at this moment +// cur = current at this moment +type Vehicle struct { + trackLen phys.Meters + vtype VehType + lights light.VehLights + + odom phys.Meters // odometer = total distance driven (runs continuously) + curPose track.Pose + curVel track.Vel + + cmdDspd phys.MetersPerSec // commanded target distance speed (eventual) + cmdDacl phys.MetersPerSec2 // commanded distance accel + desDspd phys.MetersPerSec // desired distance speed at this moment + + cmdCofs phys.Meters // commanded target center offset (eventual) + cmdCspd phys.MetersPerSec // commanded center speed (for lane change) + desCofs phys.Meters // desired center offset at this moment + + // TODO: Include fields to model [temporary] external accel? (eg centrifugal; hills; collision) + // TODO: Or, is this handled in a different part of the robotics system? +} + +// NewVehicle creates a new vehicle of the desired type. The vehicle is idle at +// the origin. +func NewVehicle(vt VehType, lspec light.Spec, trackLen phys.Meters) *Vehicle { + _, ok := vehTypeInfoTable[vt] + if !ok { + helpstr := "" + for k, v := range vehTypeInfoTable { + helpstr += fmt.Sprintf(" %s %s\n", k, v.FullName) + } + panic(fmt.Sprintf("VehType=%v is invalid. Valid vehicle types:\n%s", vt, helpstr)) + } + + return &Vehicle{ + trackLen: trackLen, + vtype: vt, + lights: *light.NewVehLights(lspec), + odom: 0, + curPose: track.Pose{Point: track.Point{Dofs: 0, Cofs: 0}, DAngle: 0}, + curVel: track.Vel{D: 0, C: 0}, + cmdDspd: 0, + cmdDacl: 0.1, + desDspd: 0, + cmdCofs: 0, + cmdCspd: 0.1, + desCofs: 0, + } +} + +// Type is the vehicle's type. +func (v *Vehicle) Type() VehType { + return v.vtype +} + +// Width is the physical width of the vehicle. +func (v *Vehicle) Width() phys.Meters { + return vehTypeInfoTable[v.vtype].Width +} + +// Length is the physical length of the vehicle. +func (v *Vehicle) Length() phys.Meters { + return vehTypeInfoTable[v.vtype].Length +} + +// Color is the vehicle's shell color +func (v *Vehicle) Color() color.Color { + return vehTypeInfoTable[v.vtype].Color +} + +// Lights returns a handle to the vehicle's lights. +func (v *Vehicle) Lights() *light.VehLights { + return &v.lights +} + +// Odom is the current odometer reading, ie total Meters driven since the +// vehicle was created. +func (v *Vehicle) Odom() phys.Meters { + return v.odom +} + +// CurTrackPose is the vehicle's current pose on the track. +func (v *Vehicle) CurTrackPose() track.Pose { + return v.curPose +} + +// CurTrackVel is the vehicle's current track velocity (V and H). +func (v *Vehicle) CurTrackVel() track.Vel { + return v.curVel +} + +// IsFacingTrackwise returns true if the vehicle is facing in the natural +// forward direction of the track. If the vehicle is not stopped, this is also +// the direcion the vehicle is driving. +// NOTE: The "trackwise" concept is akin to the "clockwise" concept. +func (v *Vehicle) IsFacingTrackwise() bool { + absDAngle := math.Abs(float64(v.curPose.DAngle)) + if absDAngle <= (math.Pi / 2) { + return true + } + if (absDAngle > (math.Pi / 2)) && (absDAngle <= math.Pi) { + return false + } + + panic(fmt.Sprintf("Vehicle.curPose.Dangle=%v is invalid!", v.curPose.DAngle)) + return false +} + +// CmdDriveDspd is the commanded distance speed, in the vehicle's driving +// direction. It is always >= 0. +func (v *Vehicle) CmdDriveDspd() phys.MetersPerSec { + return v.cmdDspd +} + +// CurDriveDspd is the current distance speed, in the vehicle's driving +// direction. It is always >= 0. +func (v *Vehicle) CurDriveDspd() phys.MetersPerSec { + return phys.MetersPerSec(math.Abs(float64(v.curVel.D))) +} + +// CmdDriveCofs is the commanded center offset, in the vehicle's driving +// direction. That is, CmdDriveCofs>0 means left-of-center in the vehicle's +// driving direction. +func (v *Vehicle) CmdDriveCofs() phys.Meters { + if v.IsFacingTrackwise() { + return v.cmdCofs + } else { + return -v.cmdCofs + } +} + +// CurDriveDofs is the current distance offset (along road center) from the +// finish line, in the vehicle's driving direction. +func (v *Vehicle) CurDriveDofs() phys.Meters { + if v.IsFacingTrackwise() { + return v.curPose.Dofs + } else { + return v.trackLen - v.curPose.Dofs + } +} + +// CurDriveDofsRem is the distance offset to the finish line (ie road center +// distance remaining), in the vehicle's driving direction. +func (v *Vehicle) CurDriveDofsRem() phys.Meters { + if v.IsFacingTrackwise() { + return v.trackLen - v.curPose.Dofs + } else { + return v.curPose.Dofs + } +} + +// CurDriveCofs is the current center offset, in the vehicle's driving +// direction. That is, CurDriveCofs>0 means left-of-center in the vehicle's +// driving direction. +func (v *Vehicle) CurDriveCofs() phys.Meters { + if v.IsFacingTrackwise() { + return v.curPose.Cofs + } else { + return -v.curPose.Cofs + } +} + +// CmdTrackCofs is the commanded center offset, in Track coordinate space, not +// the vehicle's driving direction. +func (v *Vehicle) CmdTrackCofs() phys.Meters { + return v.cmdCofs +} + +// CurTrackCofs is the current center offset, in Track coordinate space, not the +// vehicle's driving direction. +func (v *Vehicle) CurTrackCofs() phys.Meters { + return v.curPose.Cofs +} + +// Reposition manually changes position and driving direction of a vehicle, as +// if a physical vehicle was picked up and move. This changes the "commanded" +// Cofs, but does NOT change the commanded driving distance speed. +func (v *Vehicle) Reposition(p track.Pose) { + v.curPose = p + v.desCofs = p.Cofs + v.cmdCofs = p.Cofs +} + +// SetCmdDriveDspd commands a new distance speed and acceleration, in the +// vehicle's current driving direction. +func (v *Vehicle) SetCmdDriveDspd(vs phys.MetersPerSec, va phys.MetersPerSec2) { + v.cmdDspd = vs + v.cmdDacl = va +} + +// SetCmdDriveCofs commands a new center offset and speed, in the vehicle's +// current driving direction. +func (v *Vehicle) SetCmdDriveCofs(cofs phys.Meters, speed phys.MetersPerSec) { + if v.IsFacingTrackwise() { + v.cmdCofs = cofs + } else { + v.cmdCofs = -cofs + } + v.cmdCspd = speed +} + +// SetCmdTrackCofs commands a new center offset and speed. The center offset is +// absolute, in Track coordinate space. +func (v *Vehicle) SetCmdTrackCofs(cofs phys.Meters, speed phys.MetersPerSec) { + v.cmdCofs = cofs + v.cmdCspd = speed +} + +// CmdUturn commands a 180-degree uturn, toward the road center. +func (v *Vehicle) CmdUturn(radius phys.Meters) { + // XXX(gwenz): For now, uturn is instantaneous + tp := v.CurTrackPose() + if tp.Cofs < 0 { + tp.Cofs += (radius * 2) + } else { + tp.Cofs -= (radius * 2) + } + if v.IsFacingTrackwise() { + tp.DAngle = math.Pi // counter-trackwise + } else { + tp.DAngle = 0 // trackwise + } + v.Reposition(tp) +} diff --git a/goverdrive/viz/gameshape.go b/goverdrive/viz/gameshape.go new file mode 100644 index 0000000..3857cf8 --- /dev/null +++ b/goverdrive/viz/gameshape.go @@ -0,0 +1,107 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package viz + +import ( + "image/color" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo/track" +) + +const ( + // Supported GameShape types + // Note that a thick line works as a rectangle. Boo-yeah! + shapeLine = 0 + shapeCirc = 1 + numShapes = 2 +) + +// GameShape defines a flexible container for specifying primitive shapes used +// by the game. The meaning of some fields depends on specific shape. +// - Shape coordinates can be absolute or relative to a particular vehicle +// - Shape coordinates can be in Track or Cartesian coordinate space +type GameShape struct { + vehId int // >= 0 means relative to that vehicle + shape uint // eg ShapeLine + isCartes bool // coordinate space: true => cartesian; false => track + x1 phys.Meters // X or Dofs of point 1 + y1 phys.Meters // Y or Cofs of point 1 + x2 phys.Meters // X or Dofs of point 2 (may be unused, depending on Shape) + y2 phys.Meters // Y or Cofs of point 2 (may be unused, depending on Shape) + color color.Color + thickness phys.Meters // line thickness (0 => filled) +} + +func NewCartesGameLine(vehId int, p1, p2 phys.Point, color color.Color, thickness phys.Meters) *GameShape { + return &GameShape{ + vehId: vehId, + shape: shapeLine, + isCartes: true, + x1: p1.X, + y1: p1.Y, + x2: p2.X, + y2: p2.Y, + color: color, + thickness: thickness, + } +} + +func NewTrackGameLine(vehId int, tp1, tp2 track.Point, color color.Color, thickness phys.Meters) *GameShape { + return &GameShape{ + vehId: vehId, + shape: shapeLine, + isCartes: false, + x1: tp1.Dofs, + y1: tp1.Cofs, + x2: tp2.Dofs, + y2: tp2.Cofs, + color: color, + thickness: thickness, + } +} + +func NewCartesGameCirc(vehId int, ctr phys.Point, rad phys.Meters, color color.Color, thickness phys.Meters) *GameShape { + return &GameShape{ + vehId: vehId, + shape: shapeCirc, + isCartes: true, + x1: ctr.X, + y1: ctr.Y, + x2: ctr.X + rad, + y2: ctr.Y, + color: color, + thickness: thickness, + } +} + +func NewTrackGameCirc(vehId int, ctr track.Point, rad phys.Meters, color color.Color, thickness phys.Meters) *GameShape { + return &GameShape{ + vehId: vehId, + shape: shapeCirc, + isCartes: false, + x1: ctr.Dofs, + y1: ctr.Cofs, + x2: ctr.Dofs + rad, + y2: ctr.Cofs, + color: color, + thickness: thickness, + } +} + +func (gs GameShape) VehId() int { + return gs.vehId +} + +func (gs GameShape) IsCartesian() bool { + return gs.isCartes +} + +func (gs GameShape) Color() color.Color { + return gs.color +} + +func (gs GameShape) Thickness() phys.Meters { + return gs.thickness +} diff --git a/goverdrive/viz/primitive.go b/goverdrive/viz/primitive.go new file mode 100644 index 0000000..98e5bc8 --- /dev/null +++ b/goverdrive/viz/primitive.go @@ -0,0 +1,107 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +package viz + +import ( + "image/color" + + "github.com/faiface/pixel" + "github.com/faiface/pixel/imdraw" + "github.com/faiface/pixel/pixelgl" + + "github.com/anki/goverdrive/phys" +) + +// PrimitiveVisualizer provides drawing primitives whose values are in absolute +// cartesian space, using Meters. Primitives are rendered onto a canvas. +// +// Intended usage pattern: +// pv.ClearAndReset() +// pv.AddLine() +// pv.AddRectangle() +// ... // remaining shapes +// pv.RenderAll(canvas) +// // Display canvas in a window +type PrimitiveVisualizer interface { + // ClearAndReset clears all drawn shapes and resets the internal state for a + // "clean slate". + ClearAndReset() + + // RenderAll renders all of the shapes that have been added since the last + // call to ClearAndReset(). Shapes are rendered onto the passed-in canvas. + RenderAll(canvas *pixelgl.Canvas) + + // AddLine adds a line between two points + AddLine(p1, p2 phys.Point, thickness phys.Meters, clr color.Color) + + // AddRectangle adds a rectangle based on the opposite corners. When + // thickness==0, the rectangle is filled in. + AddRectangle(v1, v2 phys.Point, thickness phys.Meters, clr color.Color) + + // AddCircle adds a circle based on center point and radius. When + // thickness==0, the circle is filled in. + AddCircle(ctr phys.Point, rad phys.Meters, thickness phys.Meters, clr color.Color) + + // AddCircleArc adds circle arc based on center point and radius, and the + // beginning and end angles. When thickness==0, the circle arc is filled in. + AddCircleArc(ctr phys.Point, rad phys.Meters, begAngle phys.Radians, endAngle phys.Radians, thickness phys.Meters, clr color.Color) +} + +// XXX: The window and canvas modules think in terms of pixels, while most +// individual track and game objects are < 1.0 Meters. The primitive visualizer +// scales meters into a more usable pixel space. +const PixPerMeter float64 = 1000.0 + +////////////////////////////////////////////////////////////////////// + +// PixelViz satisfies PrimitiveVisualizer interface, using the package +// github.com/faiface/pixel. +type PixelViz struct { + imd *imdraw.IMDraw +} + +func NewPixelViz() *PixelViz { + imd := imdraw.New(nil) + return &PixelViz{imd: imd} +} + +func (pv *PixelViz) ClearAndReset() { + pv.imd.Clear() + pv.imd.Reset() +} + +func (pv *PixelViz) RenderAll(canvas *pixelgl.Canvas) { + pv.imd.Draw(canvas) +} + +// metersToPix handles conversion of units and data type casting +func metersToPix(m phys.Meters) float64 { + return PixPerMeter * float64(m) +} + +func (pv *PixelViz) AddLine(p1, p2 phys.Point, thickness phys.Meters, clr color.Color) { + pv.imd.Color = clr + pv.imd.Push(pixel.Vec{X: metersToPix(p1.X), Y: metersToPix(p1.Y)}) + pv.imd.Push(pixel.Vec{X: metersToPix(p2.X), Y: metersToPix(p2.Y)}) + pv.imd.Line(metersToPix(thickness)) +} + +func (pv *PixelViz) AddRectangle(v1, v2 phys.Point, thickness phys.Meters, clr color.Color) { + pv.imd.Color = clr + pv.imd.Push(pixel.Vec{X: metersToPix(v1.X), Y: metersToPix(v1.Y)}) + pv.imd.Push(pixel.Vec{X: metersToPix(v2.X), Y: metersToPix(v2.Y)}) + pv.imd.Rectangle(metersToPix(thickness)) +} + +func (pv *PixelViz) AddCircle(ctr phys.Point, rad phys.Meters, thickness phys.Meters, clr color.Color) { + pv.imd.Color = clr + pv.imd.Push(pixel.Vec{X: metersToPix(ctr.X), Y: metersToPix(ctr.Y)}) + pv.imd.Circle(metersToPix(rad), metersToPix(thickness)) +} + +func (pv *PixelViz) AddCircleArc(ctr phys.Point, rad phys.Meters, begAngle phys.Radians, endAngle phys.Radians, thickness phys.Meters, clr color.Color) { + pv.imd.Color = clr + pv.imd.Push(pixel.Vec{X: metersToPix(ctr.X), Y: metersToPix(ctr.Y)}) + pv.imd.CircleArc(metersToPix(rad), float64(begAngle), float64(endAngle), metersToPix(thickness)) +} diff --git a/goverdrive/viz/world.go b/goverdrive/viz/world.go new file mode 100644 index 0000000..e1b23fb --- /dev/null +++ b/goverdrive/viz/world.go @@ -0,0 +1,391 @@ +// Copyright 2017 Anki, Inc. +// Author: gwenz@anki.com + +// Package viz renders graphics, such as tracks, vehicles, and weapon effects, +// onto a canvas for visualization. It does not actually handle scaling or +// displaying the canvas in a window. +// +// Visualization features are fairly limited. Tracks, track regions, and +// vehicles are natively supported. Anything beyond this is limited to a few +// primitive geometric shapes, such as lines and circles. +package viz + +import ( + "fmt" + "image/color" + "math" + + "github.com/faiface/pixel" + "github.com/faiface/pixel/pixelgl" + "golang.org/x/image/colornames" + + "github.com/anki/goverdrive/phys" + "github.com/anki/goverdrive/robo" + "github.com/anki/goverdrive/robo/track" +) + +////////////////////////////////////////////////////////////////////// +// CONSTANTS +////////////////////////////////////////////////////////////////////// + +const ( + KTrackRegionThickness phys.Meters = 0.005 + KFinishLineThickness phys.Meters = 0.010 +) + +var ( + KTrackOutlineColor color.Color = colornames.White + KTrackCenterColor color.Color = colornames.Yellow + KTrackFinishLineColor color.Color = colornames.Lawngreen +) + +////////////////////////////////////////////////////////////////////// +// TYPES AND INTERFACES +////////////////////////////////////////////////////////////////////// + +const ( + WorldVizPadding = phys.Meters(0.08) +) + +// TrackRegion for vizualization needs a color +type TrackRegion struct { + track.Region + Color color.Color +} + +// WorldViz visualizes the objects in the goverdrive "world", such as tracks and +// vehicles. +type WorldViz interface { + // MinCorner returns the coordinate the smallest corner of the visible world + MinCorner() phys.Point + + // MaxCorner returns the coordinate the largest corner of the visible world + MaxCorner() phys.Point + + // RenderAll() renders each set of game objects onto a canvas. + // - The object sets are rendered in the order they are passed in. Ie the + // track regions are rendered before the vehicles. + // - Within an object set, objects are rendered in the order they occur + // within the slice. + RenderAll(track *track.Track, regions *[]*TrackRegion, vehs *[]robo.Vehicle, shapes *[]*GameShape) *pixelgl.Canvas +} + +////////////////////////////////////////////////////////////////////// + +// PixelWorldViz satisfies WorldViz interface, using the package +// github.com/faiface/pixel. +type PixelWorldViz struct { + pv PrimitiveVisualizer + minCorner phys.Point // minimum corner of the visible world + maxCorner phys.Point // maximum corner of the visible world + canvas *pixelgl.Canvas +} + +func NewPixelWorldViz(pv PrimitiveVisualizer, track *track.Track) *PixelWorldViz { + // add padding so track display looks nicer, and there is a little room for + // game objects, off-track driving, etc. + minCorner := track.MinCorner() + maxCorner := track.MaxCorner() + minCorner.X -= WorldVizPadding + minCorner.Y -= WorldVizPadding + maxCorner.X += WorldVizPadding + maxCorner.Y += WorldVizPadding + + return &PixelWorldViz{ + pv: pv, + minCorner: minCorner, + maxCorner: maxCorner, + canvas: nil, + } +} + +func (wv *PixelWorldViz) MinCorner() phys.Point { + return wv.minCorner +} + +func (wv *PixelWorldViz) MaxCorner() phys.Point { + return wv.maxCorner +} + +func (wv *PixelWorldViz) RenderAll(trk *track.Track, regions *[]*TrackRegion, vehs *[]robo.Vehicle, shapes *[]*GameShape) *pixelgl.Canvas { + if wv.canvas == nil { + bounds := pixel.R( + PixPerMeter*float64(wv.minCorner.X), + PixPerMeter*float64(wv.minCorner.Y), + PixPerMeter*float64(wv.maxCorner.X), + PixPerMeter*float64(wv.maxCorner.Y)) + wv.canvas = pixelgl.NewCanvas(bounds) + fmt.Printf("canvas.Bounds()=%v\n", wv.canvas.Bounds()) + } + + wv.canvas.Clear(colornames.Black) + wv.pv.ClearAndReset() + + // Track + wv.addTrack(trk) + + // Track Regions + for _, tr := range *regions { + wv.addTrackRegion(trk, tr) + } + + // Vehicles + for i, _ := range *vehs { + wv.addVehicle(i, trk, vehs) + } + + // Game Shapes + for _, shape := range *shapes { + wv.addGameShape(shape, trk, vehs) + } + + wv.pv.RenderAll(wv.canvas) + return wv.canvas +} + +// addLineAtPose adds a line between two points whose locations are relative to +// the position + rotation of a pose. +func (wv *PixelWorldViz) addLineAtPose(p phys.Pose, p1, p2 phys.Point, thickness phys.Meters, clr color.Color) { + pp1 := p1.ToPolarPoint() + pp2 := p2.ToPolarPoint() + + // p.A == 0 means facing +X direction + pp1.A += p.Theta + pp2.A += p.Theta + + p1 = pp1.ToPoint() + p2 = pp2.ToPoint() + + p1.X += p.X + p1.Y += p.Y + p2.X += p.X + p2.Y += p.Y + + wv.pv.AddLine(p1, p2, thickness, clr) +} + +////////////////////////////////////////////////////////////////////// + +// addRoadPieceDLine renders a "distance" line at a specified center offset, on +// a particular road piece. It is required that: +// 1. begDofs >= 0 +// 2. endDofs > begDofs +// 3. endDofs <= rp.CenLen() +func (wv *PixelWorldViz) addRoadPieceDLine(trk *track.Track, rpi track.Rpi, cofs, begDofs, endDofs, thickness phys.Meters, clr color.Color) { + rp := trk.Rp(rpi) + if begDofs < 0 { + panic(fmt.Sprintf("addRoadPieceDLine requires (begDofs >= 0), but begVof =%v", begDofs)) + } + if begDofs >= endDofs { + panic(fmt.Sprintf("addRoadPieceDLine requires (endDofs > begDofs), but begDofs=%v, endDofs=%v", begDofs, endDofs)) + } + if endDofs > rp.CenLen() { + panic(fmt.Sprintf("addRoadPieceDLine requires (endDofs <= rp.CenLen()), but endDofs=%v, rp.CenLen()=%v", endDofs, rp.CenLen())) + } + + if rp.IsStraight() { + p1 := phys.Point{X: begDofs, Y: cofs} + p2 := phys.Point{X: endDofs, Y: cofs} + wv.addLineAtPose(trk.RpEntryPose(rpi), p1, p2, thickness, clr) + } else { // curved road piece + // compute absolute beg/end angles of the circle arc + rad := rp.CurveRadius(cofs) + ctr := trk.RpCurveCenter(rpi) + angles := make([]phys.Radians, 2, 2) + for i := 0; i < 2; i++ { + pose := trk.RpEntryPose(rpi + track.Rpi(i)) + pp := phys.Point{X: pose.X - ctr.X, Y: pose.Y - ctr.Y}.ToPolarPoint() + angles[i] = pp.A + } + if (rp.DAngle() > 0) && (angles[1] < angles[0]) { + angles[1] += (2 * math.Pi) + } + if (rp.DAngle() < 0) && (angles[1] > angles[0]) { + angles[1] -= (2 * math.Pi) + } + // angles[] now has the beg/end angles for drawing a vert line for the WHOLE + // piece. Need to adjust for begCofs/endCofs. + dAngle := angles[1] - angles[0] + angles[1] = angles[0] + (dAngle * phys.Radians(endDofs/rp.CenLen())) + angles[0] = angles[0] + (dAngle * phys.Radians(begDofs/rp.CenLen())) + //fmt.Printf("rpi=%v: angles[0]=%v, angles[1]=%v\n", rpi, angles[0], angles[1]) + wv.pv.AddCircleArc(ctr, rad, angles[0], angles[1], thickness, clr) + } +} + +// addTrackDLine renders a "distance" line at a specified center offset on the +// track. The line follows the curvature of the track. When (dofs1 > dofs2), the +// distance line will cross the finish line. +func (wv *PixelWorldViz) addTrackDLine(trk *track.Track, cofs, dofs1, dofs2, thickness phys.Meters, clr color.Color) { + rpi1, rpDofs1 := trk.RpiAndRpDofs(dofs1) + rpi2, rpDofs2 := trk.RpiAndRpDofs(dofs2) + if rpi1 == rpi2 { + wv.addRoadPieceDLine(trk, rpi1, cofs, rpDofs1, rpDofs2, thickness, clr) + return + } + rpCount := int(rpi2 - rpi1) + if rpi2 < rpi1 { + rpCount += trk.NumRp() + } + for i := 0; i < rpCount; i++ { + rpi := rpi1 + track.Rpi(i) + if int(rpi) >= trk.NumRp() { + rpi -= track.Rpi(trk.NumRp()) + } + rp := trk.Rp(rpi) + //fmt.Printf("addTrackDLine(rpi=%v): cofs=%v, dofs1=%v, vof2=%v, rpDofs1=%v, rpDofs2=%v, rp.CenLen()=%v\n", rpi, cofs, dofs1, dofs2, rpDofs1, rpDofs2, rp.CenLen()) + if rpi == rpi1 { + wv.addRoadPieceDLine(trk, rpi, cofs, rpDofs1, rp.CenLen(), thickness, clr) + } else { + wv.addRoadPieceDLine(trk, rpi, cofs, 0, rp.CenLen(), thickness, clr) + } + } + if rpDofs2 > 1.0e-6 { + wv.addRoadPieceDLine(trk, rpi2, cofs, 0, rpDofs2, thickness, clr) + } +} + +// addTrackCLine renders a "center offset" line at a specified distance offset +// on the track. +func (wv *PixelWorldViz) addTrackCLine(trk *track.Track, dofs, cofs1, cofs2, thickness phys.Meters, clr color.Color) { + pose := trk.ToPose(track.Pose{Point: track.Point{Dofs: dofs, Cofs: 0}, DAngle: 0}) + + // line endpoints are relative to the pose of the track + p1 := phys.Point{X: 0, Y: cofs1} + p2 := phys.Point{X: 0, Y: cofs2} + + wv.addLineAtPose(pose, p1, p2, thickness, clr) +} + +// addTrackRegion renders an unfilled track region which bends to the shape of +// the track. +func (wv *PixelWorldViz) addTrackRegion(track *track.Track, tr *TrackRegion) { + if tr.Len() >= track.CenLen() { + // XXX(gwenz): This avoids crashes and incorrectly rendered track regions. + // Probably it should not be necessary, with proper rendering algorithms. + wv.addTrackDLine(track, tr.C1().Cofs, 0, track.CenLen(), KTrackRegionThickness, tr.Color) + wv.addTrackDLine(track, tr.C2().Cofs, 0, track.CenLen(), KTrackRegionThickness, tr.Color) + return + } + wv.addTrackCLine(track, tr.C1().Dofs, tr.C1().Cofs, tr.C2().Cofs, KTrackRegionThickness, tr.Color) + wv.addTrackCLine(track, tr.C2().Dofs, tr.C1().Cofs, tr.C2().Cofs, KTrackRegionThickness, tr.Color) + wv.addTrackDLine(track, tr.C1().Cofs, tr.C1().Dofs, tr.C2().Dofs, KTrackRegionThickness, tr.Color) + wv.addTrackDLine(track, tr.C2().Cofs, tr.C1().Dofs, tr.C2().Dofs, KTrackRegionThickness, tr.Color) +} + +// addTrack performs the individual commands to render an entire track. +func (wv *PixelWorldViz) addTrack(trk *track.Track) { + // finish line + flL := phys.Point{X: 0, Y: +trk.Width() / 2} + flR := phys.Point{X: 0, Y: -trk.Width() / 2} + flTrackPose := track.Pose{Point: track.Point{Dofs: track.TrackLenModStartShort, Cofs: 0}, DAngle: 0} + flPoint := trk.ToPose(flTrackPose).Point + wv.pv.AddLine(flL, flPoint, KFinishLineThickness/3, KTrackFinishLineColor) + wv.pv.AddLine(flR, flPoint, KFinishLineThickness/3, KTrackFinishLineColor) + wv.addTrackCLine(trk, 0, -trk.Width()/2, +trk.Width()/2, KFinishLineThickness, KTrackFinishLineColor) + + // Easiest way to render is to make each road piece a single track region. + for rpi := track.Rpi(0); rpi < track.Rpi(trk.NumRp()); rpi++ { + rp := trk.Rp(rpi) + cenLen := rp.CenLen() + + centerTrC1 := track.Point{Dofs: trk.RpEntryDofs(rpi), Cofs: 0} + centerTr := track.NewRegion(trk, centerTrC1, cenLen, 0.0001) + centerRegion := TrackRegion{ + Region: *centerTr, + Color: KTrackCenterColor, + } + wv.addTrackRegion(trk, ¢erRegion) + + outlineTrC1 := track.Point{Dofs: trk.RpEntryDofs(rpi), Cofs: -trk.Width() / 2} + outlineTr := track.NewRegion(trk, outlineTrC1, cenLen, trk.Width()) + outlineRegion := TrackRegion{ + Region: *outlineTr, + Color: KTrackOutlineColor, + } + wv.addTrackRegion(trk, &outlineRegion) + } +} + +// addVehicle renders a vehicle at its position on the track +func (wv *PixelWorldViz) addVehicle(vehId int, track *track.Track, vehs *[]robo.Vehicle) { + v := &(*vehs)[vehId] + // car body = colored rectangle + wv.addLineAtPose(track.ToPose(v.CurTrackPose()), + phys.Point{X: -(v.Length() / 2), Y: 0}, + phys.Point{X: +(v.Length() / 2), Y: 0}, + v.Width(), v.Color()) + // lights = filled circles + for _, lvi := range v.Lights().VizInfo() { + gs := NewCartesGameCirc(vehId, phys.Point{X: lvi.X, Y: lvi.Y}, lvi.R, lvi.Color, 0) + wv.addGameShape(gs, track, vehs) + } +} + +// addGameShape renders the appropriate game shape +func (wv *PixelWorldViz) addGameShape(gs *GameShape, trk *track.Track, vehs *[]robo.Vehicle) { + if (gs.VehId() >= 0) && (gs.VehId() >= len(*vehs)) { + panic(fmt.Sprintf("PixelWorldViz.addGameShape() with VehdId=%d is invalid; game only has %d vehicles!", gs.VehId(), len(*vehs))) + } + if gs.shape >= numShapes { + panic(fmt.Sprintf("PixelWorldViz.addGameShape: gs.shape=%v is invalid", gs.shape)) + } + + if (gs.VehId() >= 0) && gs.IsCartesian() { + // shape's position is relative to vehicle's pose, in Cartesian coordinate space + pose := trk.ToPose((*vehs)[gs.VehId()].CurTrackPose()) + pose1 := pose.AdvancePose(phys.Pose{Point: phys.Point{X: gs.x1, Y: gs.y1}, Theta: 0}) + pose2 := pose.AdvancePose(phys.Pose{Point: phys.Point{X: gs.x2, Y: gs.y2}, Theta: 0}) + + p1 := phys.Point{X: pose1.X, Y: pose1.Y} + p2 := phys.Point{X: pose2.X, Y: pose2.Y} + switch gs.shape { + case shapeLine: + wv.pv.AddLine(p1, p2, gs.Thickness(), gs.Color()) + case shapeCirc: + radius := phys.Dist(p1, p2) + wv.pv.AddCircle(p1, radius, gs.Thickness(), gs.Color()) + } + } else if (gs.VehId() >= 0) && !gs.IsCartesian() { + // shape's position is relative to vehicle's pose, in Track coordinate space + vtp := (*vehs)[gs.VehId()].CurTrackPose() + tp1 := track.Pose{Point: track.Point{Dofs: gs.x1 + vtp.Dofs, Cofs: gs.y1 + vtp.Cofs}, DAngle: 0} + tp2 := track.Pose{Point: track.Point{Dofs: gs.x2 + vtp.Dofs, Cofs: gs.y2 + vtp.Cofs}, DAngle: 0} + tp1.Dofs = trk.NormalizeDofs(tp1.Dofs) + tp2.Dofs = trk.NormalizeDofs(tp2.Dofs) + pose1 := trk.ToPose(tp1) + pose2 := trk.ToPose(tp2) + p1 := phys.Point{X: pose1.X, Y: pose1.Y} + p2 := phys.Point{X: pose2.X, Y: pose2.Y} + switch gs.shape { + case shapeLine: + wv.addTrackDLine(trk, tp1.Cofs, tp1.Dofs, tp2.Dofs, gs.Thickness(), gs.Color()) + //wv.pv.AddLine(p1, p2, gs.Thickness(), gs.Color()) + case shapeCirc: + radius := phys.Dist(p1, p2) + wv.pv.AddCircle(p1, radius, gs.Thickness(), gs.Color()) + } + } else { + // shape's position is absolute + var pose1 phys.Pose + var pose2 phys.Pose + if gs.IsCartesian() { + // Cartesian coordinate space + pose1 = phys.Pose{Point: phys.Point{X: gs.x1, Y: gs.y1}, Theta: 0} + pose2 = phys.Pose{Point: phys.Point{X: gs.x2, Y: gs.y2}, Theta: 0} + } else { + // Track coordinate space + pose1 = trk.ToPose(track.Pose{Point: track.Point{Dofs: gs.x1, Cofs: gs.y1}, DAngle: 0}) + pose2 = trk.ToPose(track.Pose{Point: track.Point{Dofs: gs.x2, Cofs: gs.y2}, DAngle: 0}) + } + p1 := phys.Point{X: pose1.X, Y: pose1.Y} + p2 := phys.Point{X: pose2.X, Y: pose2.Y} + switch gs.shape { + case shapeLine: + wv.pv.AddLine(p1, p2, gs.Thickness(), gs.Color()) + case shapeCirc: + radius := phys.Dist(p1, p2) + wv.pv.AddCircle(p1, radius, gs.Thickness(), gs.Color()) + } + } +}