Skip to content

Commit

Permalink
Fix api
Browse files Browse the repository at this point in the history
  • Loading branch information
@johnfercher
johnfercher committed Sep 9, 2023
1 parent 0b8f4cc commit 3bf046f
Show file tree
Hide file tree
Showing 3 changed files with 54 additions and 44 deletions.
45 changes: 19 additions & 26 deletions cmd/main.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -11,8 +11,8 @@ func main() {
stepDistance := 0.1
r := rrt.New[string](stepDistance)

r.AddCollisionCondition(func(point *rrt.Point[string]) bool {
return point.Data == "obstacle"
r.AddCollisionCondition(func(point string) bool {
return point == "obstacle"
})

r.AddStopCondition(func(testPoint *rrt.Point[string], finish *rrt.Point[string]) bool {
Expand All @@ -22,38 +22,31 @@ func main() {
space := generateClearSpace(128, 128)
space = addObstacles(5, 3, space)

start, finish, err := getStartAndFinishPoints(space)
start, finish, err := getStartAndFinishCoordinate(space)
if err != nil {
panic(err)
}

start.Println()
finish.Println()
points := r.FindPath(start, finish, space)
for _, point := range points {
point.Println()
}
}

func generateClearSpace[T string](x, y int) [][]*rrt.Point[T] {
var space [][]*rrt.Point[T]
func generateClearSpace[T string](x, y int) [][]T {
var space [][]T
for i := 0; i < x; i++ {
var line []*rrt.Point[T]
var line []T
for j := 0; j < y; j++ {
point := &rrt.Point[T]{
X: float64(i),
Y: float64(j),
Data: "empty",
}
line = append(line, point)
line = append(line, "empty")
}
space = append(space, line)
}

return space
}

func addObstacles[T string](qtd, size int, space [][]*rrt.Point[T]) [][]*rrt.Point[T] {
func addObstacles[T string](qtd, size int, space [][]T) [][]T {
for i := 0; i < qtd; i++ {
x := rand.Int()%len(space) - 1
y := rand.Int()%len(space[0]) - 1
Expand All @@ -64,7 +57,7 @@ func addObstacles[T string](qtd, size int, space [][]*rrt.Point[T]) [][]*rrt.Poi
return space
}

func addObstacle[T string](x, y, size int, space [][]*rrt.Point[T]) [][]*rrt.Point[T] {
func addObstacle[T string](x, y, size int, space [][]T) [][]T {
offset := (size - 1) / 2
//fmt.Println(offset)

Expand Down Expand Up @@ -94,17 +87,17 @@ func addObstacle[T string](x, y, size int, space [][]*rrt.Point[T]) [][]*rrt.Poi
for i := minXOffset; i <= maxXOffset; i++ {
for j := minYOffset; j <= maxYOffset; j++ {
//fmt.Printf("%d - %d\n", i, j)
space[i][j].Data = "obstacle"
space[i][j] = "obstacle"
}
}

return space
}

func getStartAndFinishPoints[T string](space [][]*rrt.Point[T]) (*rrt.Point[T], *rrt.Point[T], error) {
func getStartAndFinishCoordinate[T string](space [][]T) (*rrt.Coordinate, *rrt.Coordinate, error) {
tries := 10
start := &rrt.Point[T]{}
finish := &rrt.Point[T]{}
var start *rrt.Coordinate
var finish *rrt.Coordinate

found := false
for i := 0; i < tries && !found; i++ {
Expand All @@ -113,9 +106,9 @@ func getStartAndFinishPoints[T string](space [][]*rrt.Point[T]) (*rrt.Point[T],

//fmt.Printf("%d - %d\n", x, y)

if space[x][y].Data != "obstacle" {
space[x][y].Data = "start"
start = space[x][y]
if space[x][y] != "obstacle" {
space[x][y] = "start"
start = &rrt.Coordinate{X: float64(x), Y: float64(y)}
found = true
}
}
Expand All @@ -130,9 +123,9 @@ func getStartAndFinishPoints[T string](space [][]*rrt.Point[T]) (*rrt.Point[T],

//fmt.Printf("%d - %d\n", x, y)

if space[x][y].Data != "obstacle" && space[x][y].Data != "start" {
space[x][y].Data = "finish"
finish = space[x][y]
if space[x][y] != "obstacle" && space[x][y] != "start" {
space[x][y] = "finish"
finish = &rrt.Coordinate{X: float64(x), Y: float64(y)}
found = true
}
}
Expand Down
5 changes: 5 additions & 0 deletions pkg/rrt/point.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -5,6 +5,11 @@ import (
"math"
)

type Coordinate struct {
X float64
Y float64
}

type Point[T any] struct {
X float64
Y float64
Expand Down
48 changes: 30 additions & 18 deletions pkg/rrt/rrt.go
View file
Original file line number Diff line number Diff line change
Expand Up @@ -8,11 +8,11 @@ import (
)

type RRT[T any] struct {
collisionCondition func(point *Point[T]) bool
collisionCondition func(point T) bool
stopCondition func(testPoint *Point[T], finish *Point[T]) bool
stepDistance float64
start *Point[T]
finish *Point[T]
startPoint *Point[T]
finishPoint *Point[T]
}

func New[T any](stepDistance float64) *RRT[T] {
Expand All @@ -21,7 +21,7 @@ func New[T any](stepDistance float64) *RRT[T] {
}
}

func (r *RRT[T]) AddCollisionCondition(condition func(point *Point[T]) bool) *RRT[T] {
func (r *RRT[T]) AddCollisionCondition(condition func(point T) bool) *RRT[T] {
r.collisionCondition = condition
return r
}
Expand All @@ -31,18 +31,25 @@ func (r *RRT[T]) AddStopCondition(condition func(testPoint *Point[T], finish *Po
return r
}

func (r *RRT[T]) FindPath(start *Point[T], finish *Point[T], world [][]*Point[T]) []*Point[T] {
r.start = start
r.finish = finish
func (r *RRT[T]) FindPath(start *Coordinate, finish *Coordinate, world [][]T) []*Point[T] {
r.startPoint = &Point[T]{
X: start.X,
Y: start.Y,
}
r.finishPoint = &Point[T]{
X: finish.X,
Y: finish.Y,
}

var nodesArray []*tree.Node[*Point[T]]
tr := tree.New[*Point[T]]()

maxDistance := Distance(world[0][0], world[len(world)-1][len(world[0])-1])
maxDistance := Distance(&Point[T]{X: 0, Y: 0}, &Point[T]{X: float64(len(world) - 1), Y: float64(len(world[0]) - 1)})

fmt.Printf("Max Distance: %f\n", maxDistance)

nodeCounter := 0
node := tree.NewNode(nodeCounter, start)
node := tree.NewNode[*Point[T]](nodeCounter, r.startPoint)
tr.AddRoot(node)
nodesArray = append(nodesArray, node)

Expand All @@ -59,7 +66,7 @@ func (r *RRT[T]) FindPath(start *Point[T], finish *Point[T], world [][]*Point[T]
goTofinish = !goTofinish

_, lastAdded := nodesArray[len(nodesArray)-1].Get()
if r.stopCondition(lastAdded, finish) {
if r.stopCondition(lastAdded, r.finishPoint) {
break
}

Expand All @@ -75,7 +82,7 @@ func (r *RRT[T]) FindPath(start *Point[T], finish *Point[T], world [][]*Point[T]

fixedPoint := r.getFixedPoint(minDistancePoint, newPoint, world)

if !r.collisionCondition(fixedPoint) {
if !r.collisionCondition(fixedPoint.Data) {
nodeCounter++
newNode := tree.NewNode(nodeCounter, fixedPoint)
ok := tr.Add(nodeCounter-1, newNode)
Expand All @@ -96,9 +103,9 @@ func (r *RRT[T]) FindPath(start *Point[T], finish *Point[T], world [][]*Point[T]
return points
}

func (r *RRT[T]) getRandomPoint(world [][]*Point[T], goToFinish bool) *Point[T] {
func (r *RRT[T]) getRandomPoint(world [][]T, goToFinish bool) *Point[T] {
if goToFinish {
return r.finish
return r.finishPoint
}

x := rand.Int() % len(world)
Expand All @@ -111,11 +118,14 @@ func (r *RRT[T]) getRandomPoint(world [][]*Point[T], goToFinish bool) *Point[T]
if y > len(world[0])-1 {
y = len(world[0]) - 1
}
//fmt.Printf("%d, %d\n", x, y)
return world[x][y]

return &Point[T]{
X: float64(x),
Y: float64(y),
}
}

func (r *RRT[T]) getFixedPoint(minDistancePoint *Point[T], newPoint *Point[T], world [][]*Point[T]) *Point[T] {
func (r *RRT[T]) getFixedPoint(minDistancePoint *Point[T], newPoint *Point[T], world [][]T) *Point[T] {

radian := Radian(minDistancePoint, newPoint)
x := int(minDistancePoint.X + (math.Sin(radian)*minDistancePoint.X)*r.stepDistance)
Expand All @@ -130,6 +140,8 @@ func (r *RRT[T]) getFixedPoint(minDistancePoint *Point[T], newPoint *Point[T], w
}

//fmt.Printf("Min %s, New %s, Fix %s\n", minDistancePoint.GetString(), newPoint.GetString(), world[x][y].GetString())

return world[x][y]
return &Point[T]{
X: float64(x),
Y: float64(y),
}
}

0 comments on commit 3bf046f

Please sign in to comment.