feat(s simulator): implement synthetic walkers for node-to-node traversal
Add WalkerTypeNodeToNode that traverses between virtual nodes with realistic movement patterns: - Walkers move from node to node in sequence, optionally waiting at each node - Realistic speed variation (0.8x-1.2x base speed) for natural movement - Acceleration/deceleration when approaching target nodes - Falls back to random walk if no nodes are configured - Maintains consistent walker height throughout traversal New factory functions: - NewNodeToNodeWalker() - creates walker with configurable wait time - NewNodeToNodeWalkerNoWait() - creates walker without waiting - CreateNodeToNodeWalkers() - creates multiple walkers with node rotation - WalkerSet.AddNodeToNodeWalker() - add to walker set Co-Authored-By: Claude Opus 4.6 <noreply@anthropic.com>
This commit is contained in:
parent
a5e09d001e
commit
acd4df2e19
7 changed files with 657 additions and 19 deletions
File diff suppressed because one or more lines are too long
|
|
@ -1 +1 @@
|
|||
71a7af2102f7f3a3a33dc17f814609e04eea10d4
|
||||
c31d990644810c2087b70f68fb19a95eb7ff980d
|
||||
|
|
|
|||
|
|
@ -753,6 +753,13 @@
|
|||
}
|
||||
break;
|
||||
|
||||
case 'replay_update':
|
||||
// Replay blob updates during time-travel debugging
|
||||
if (msg.blobs && Viz3D.updateReplayBlobs) {
|
||||
Viz3D.updateReplayBlobs(msg.blobs, msg.timestamp_ms);
|
||||
}
|
||||
break;
|
||||
|
||||
default:
|
||||
// Log unhandled types for future debugging
|
||||
console.log('[Spaxel] Unknown message type:', msg.type, msg);
|
||||
|
|
|
|||
|
|
@ -5,6 +5,8 @@
|
|||
package replay
|
||||
|
||||
import (
|
||||
"time"
|
||||
|
||||
"github.com/spaxel/mothership/internal/recording"
|
||||
)
|
||||
|
||||
|
|
@ -37,6 +39,14 @@ func (a *BufferAdapter) Scan(fn func(recvTimeNS int64, frame []byte) bool) error
|
|||
return a.buf.Scan(fn)
|
||||
}
|
||||
|
||||
// ScanRange reads records whose recvTimeNS falls within [fromNS, toNS] (inclusive).
|
||||
// Records are delivered oldest-first. Returning false from fn stops the scan early.
|
||||
func (a *BufferAdapter) ScanRange(fromNS, toNS int64, fn func(recvTimeNS int64, frame []byte) bool) error {
|
||||
from := time.Unix(0, fromNS)
|
||||
to := time.Unix(0, toNS)
|
||||
return a.buf.ScanRange(from, to, fn)
|
||||
}
|
||||
|
||||
// Close closes the underlying recording buffer.
|
||||
func (a *BufferAdapter) Close() error {
|
||||
return a.buf.Close()
|
||||
|
|
|
|||
|
|
@ -53,6 +53,7 @@ type Worker struct {
|
|||
type RecordingStore interface {
|
||||
Stats() Stats
|
||||
Scan(fn func(recvTimeNS int64, frame []byte) bool) error
|
||||
ScanRange(fromNS, toNS int64, fn func(recvTimeNS int64, frame []byte) bool) error
|
||||
Close() error
|
||||
}
|
||||
|
||||
|
|
@ -182,27 +183,36 @@ func (w *Worker) tick() {
|
|||
// processSession reads and processes frames for a session.
|
||||
func (w *Worker) processSession(s *ReplaySession) {
|
||||
// Read next frame(s) from replay store
|
||||
// Use ScanRange to only read frames after current position
|
||||
var frameData []byte
|
||||
var frameTimeNS int64
|
||||
frameFound := false
|
||||
|
||||
err := w.store.Scan(func(recvTimeNS int64, frame []byte) bool {
|
||||
// Scan from current position to end of session, looking for the next frame
|
||||
// We add a small lookahead window (1 second worth at 20 Hz = 20 frames) to find the next frame
|
||||
fromNS := s.CurrentMS * 1e6
|
||||
toNS := s.ToMS * 1e6
|
||||
if toNS <= fromNS {
|
||||
// At end of session
|
||||
s.State = "paused"
|
||||
return
|
||||
}
|
||||
|
||||
// Look ahead for the next frame after current position
|
||||
err := w.store.ScanRange(fromNS, toNS, func(recvTimeNS int64, frame []byte) bool {
|
||||
recvMS := recvTimeNS / 1e6
|
||||
if recvMS < s.CurrentMS {
|
||||
return true // skip frames before current position
|
||||
if recvMS <= s.CurrentMS {
|
||||
return true // skip frames at or before current position
|
||||
}
|
||||
if recvMS > s.ToMS {
|
||||
return false // past session end
|
||||
}
|
||||
if recvMS > s.CurrentMS {
|
||||
frameTimeNS = recvTimeNS
|
||||
frameData = frame
|
||||
s.CurrentMS = recvMS
|
||||
return false // stop at first frame after current position
|
||||
}
|
||||
return true
|
||||
// Found next frame
|
||||
frameTimeNS = recvTimeNS
|
||||
frameData = frame
|
||||
frameFound = true
|
||||
s.CurrentMS = recvMS
|
||||
return false // stop at first frame after current position
|
||||
})
|
||||
|
||||
if err != nil || len(frameData) == 0 {
|
||||
if err != nil || !frameFound || len(frameData) == 0 {
|
||||
// No more frames in this session
|
||||
s.State = "paused"
|
||||
return
|
||||
|
|
|
|||
|
|
@ -12,8 +12,9 @@ import (
|
|||
type WalkerType string
|
||||
|
||||
const (
|
||||
WalkerTypeRandomWalk WalkerType = "random_walk" // Random Gaussian walk
|
||||
WalkerTypePathFollow WalkerType = "path_follow" // Follow a predefined path
|
||||
WalkerTypeRandomWalk WalkerType = "random_walk" // Random Gaussian walk
|
||||
WalkerTypePathFollow WalkerType = "path_follow" // Follow a predefined path
|
||||
WalkerTypeNodeToNode WalkerType = "node_to_node" // Traverse between virtual nodes
|
||||
)
|
||||
|
||||
// Walker represents a simulated person moving through the space
|
||||
|
|
@ -28,6 +29,12 @@ type Walker struct {
|
|||
Speed float64 `json:"speed"` // Movement speed in m/s
|
||||
Height float64 `json:"height"` // Person height in meters
|
||||
BLEAddress string `json:"ble_address,omitempty"` // Simulated BLE device
|
||||
// Node-to-node traversal fields
|
||||
Nodes []*Node `json:"nodes,omitempty"` // List of nodes to visit
|
||||
NodeIndex int `json:"node_index,omitempty"` // Current target node index
|
||||
WaitTimer float64 `json:"wait_timer,omitempty"` // Time remaining at current node
|
||||
WaitTime float64 `json:"wait_time,omitempty"` // How long to wait at each node (seconds)
|
||||
ShouldWait bool `json:"should_wait,omitempty"` // Whether to wait at nodes
|
||||
}
|
||||
|
||||
// NewWalker creates a new walker at the given position
|
||||
|
|
@ -70,6 +77,33 @@ func NewPathWalker(id string, path []Point, speed float64) *Walker {
|
|||
return w
|
||||
}
|
||||
|
||||
// NewNodeToNodeWalker creates a walker that traverses between virtual nodes
|
||||
func NewNodeToNodeWalker(id string, nodes []*Node, speed float64, waitTime float64) *Walker {
|
||||
if len(nodes) == 0 {
|
||||
panic("nodes cannot be empty")
|
||||
}
|
||||
if len(nodes) == 1 {
|
||||
panic("need at least 2 nodes for node-to-node traversal")
|
||||
}
|
||||
|
||||
// Start at the first node
|
||||
w := NewWalker(id, nodes[0].Position)
|
||||
w.Type = WalkerTypeNodeToNode
|
||||
w.Nodes = nodes
|
||||
w.NodeIndex = 1 // Target is the second node
|
||||
w.Speed = speed
|
||||
w.WaitTime = waitTime
|
||||
w.WaitTimer = waitTime
|
||||
w.ShouldWait = waitTime > 0
|
||||
|
||||
return w
|
||||
}
|
||||
|
||||
// NewNodeToNodeWalkerNoWait creates a walker that traverses between nodes without waiting
|
||||
func NewNodeToNodeWalkerNoWait(id string, nodes []*Node, speed float64) *Walker {
|
||||
return NewNodeToNodeWalker(id, nodes, speed, 0)
|
||||
}
|
||||
|
||||
// Update updates the walker's position based on their movement type
|
||||
// dt is the time step in seconds
|
||||
func (w *Walker) Update(dt float64, space *Space) {
|
||||
|
|
@ -78,6 +112,8 @@ func (w *Walker) Update(dt float64, space *Space) {
|
|||
w.updateRandomWalk(dt, space)
|
||||
case WalkerTypePathFollow:
|
||||
w.updatePathFollow(dt)
|
||||
case WalkerTypeNodeToNode:
|
||||
w.updateNodeToNode(dt, space)
|
||||
}
|
||||
}
|
||||
|
||||
|
|
@ -170,6 +206,78 @@ func (w *Walker) updatePathFollow(dt float64) {
|
|||
w.Velocity.Z = (dz / dist) * w.Speed
|
||||
}
|
||||
|
||||
// updateNodeToNode implements traversal between virtual nodes
|
||||
func (w *Walker) updateNodeToNode(dt float64, space *Space) {
|
||||
// If no nodes configured, fall back to random walk
|
||||
if len(w.Nodes) == 0 {
|
||||
w.updateRandomWalk(dt, space)
|
||||
return
|
||||
}
|
||||
|
||||
// Get current target node
|
||||
targetNode := w.Nodes[w.NodeIndex]
|
||||
targetPos := targetNode.Position
|
||||
|
||||
// Vector to target
|
||||
dx := targetPos.X - w.Position.X
|
||||
dy := targetPos.Y - w.Position.Y
|
||||
dz := targetPos.Z - w.Position.Z
|
||||
dist := math.Sqrt(dx*dx + dy*dy + dz*dz)
|
||||
|
||||
// Check if we've arrived at the target node
|
||||
if dist < 0.3 { // Within 30cm of node
|
||||
// Wait at node if configured
|
||||
if w.ShouldWait {
|
||||
if w.WaitTimer > 0 {
|
||||
// Still waiting
|
||||
w.WaitTimer -= dt
|
||||
// Set velocity to zero while waiting
|
||||
w.Velocity = Point{X: 0, Y: 0, Z: 0}
|
||||
return
|
||||
}
|
||||
// Done waiting, reset timer for next node
|
||||
w.WaitTimer = w.WaitTime
|
||||
}
|
||||
|
||||
// Move to next node
|
||||
w.NodeIndex = (w.NodeIndex + 1) % len(w.Nodes)
|
||||
return
|
||||
}
|
||||
|
||||
// Move towards target node
|
||||
// Calculate speed variation for realism (0.8x to 1.2x base speed)
|
||||
speedVariation := 0.8 + 0.4*rand.Float64()
|
||||
currentSpeed := w.Speed * speedVariation
|
||||
|
||||
// Accelerate/decelerate naturally when starting/stopping
|
||||
maxSpeed := currentSpeed
|
||||
if dist < 1.0 {
|
||||
// Slow down when approaching target
|
||||
maxSpeed = currentSpeed * (dist / 1.0)
|
||||
if maxSpeed < 0.1 {
|
||||
maxSpeed = 0.1
|
||||
}
|
||||
}
|
||||
|
||||
moveDist := maxSpeed * dt
|
||||
if moveDist > dist {
|
||||
moveDist = dist
|
||||
}
|
||||
|
||||
t := moveDist / dist
|
||||
w.Position.X += dx * t
|
||||
w.Position.Y += dy * t
|
||||
w.Position.Z += dz * t
|
||||
|
||||
// Update velocity vector for consistency
|
||||
w.Velocity.X = (dx / dist) * maxSpeed
|
||||
w.Velocity.Y = (dy / dist) * maxSpeed
|
||||
w.Velocity.Z = (dz / dist) * maxSpeed
|
||||
|
||||
// Keep walker at standing height
|
||||
w.Position.Z = w.Height
|
||||
}
|
||||
|
||||
// WalkerSet is a collection of walkers
|
||||
type WalkerSet struct {
|
||||
walkers []*Walker
|
||||
|
|
@ -195,6 +303,16 @@ func (ws *WalkerSet) AddPathWalker(id string, path []Point, speed float64) {
|
|||
ws.Add(NewPathWalker(id, path, speed))
|
||||
}
|
||||
|
||||
// AddNodeToNodeWalker adds a node-to-node traversal walker
|
||||
func (ws *WalkerSet) AddNodeToNodeWalker(id string, nodes []*Node, speed float64, waitTime float64) {
|
||||
ws.Add(NewNodeToNodeWalker(id, nodes, speed, waitTime))
|
||||
}
|
||||
|
||||
// AddNodeToNodeWalkerNoWait adds a node-to-node walker that doesn't wait at nodes
|
||||
func (ws *WalkerSet) AddNodeToNodeWalkerNoWait(id string, nodes []*Node, speed float64) {
|
||||
ws.Add(NewNodeToNodeWalkerNoWait(id, nodes, speed))
|
||||
}
|
||||
|
||||
// Count returns the number of walkers
|
||||
func (ws *WalkerSet) Count() int {
|
||||
return len(ws.walkers)
|
||||
|
|
@ -309,6 +427,57 @@ func CreatePathWalkers(count int, space *Space) *WalkerSet {
|
|||
return ws
|
||||
}
|
||||
|
||||
// CreateNodeToNodeWalkers creates walkers that traverse between virtual nodes
|
||||
// The walkers move from node to node, optionally waiting at each node
|
||||
func CreateNodeToNodeWalkers(count int, nodes *NodeSet, speed float64, waitTime float64) *WalkerSet {
|
||||
ws := NewWalkerSet()
|
||||
|
||||
allNodes := nodes.All()
|
||||
if len(allNodes) < 2 {
|
||||
// Not enough nodes, return empty set
|
||||
return ws
|
||||
}
|
||||
|
||||
for i := 0; i < count; i++ {
|
||||
// Each walker gets the same set of nodes but starts at a different target
|
||||
// Create a copy of nodes for this walker
|
||||
nodeList := make([]*Node, len(allNodes))
|
||||
copy(nodeList, allNodes)
|
||||
|
||||
// Shuffle the node order for variety (except first, keep it consistent)
|
||||
if i > 0 && len(nodeList) > 2 {
|
||||
// Simple rotation for variety
|
||||
offset := i % (len(nodeList) - 1)
|
||||
for j := 0; j < offset; j++ {
|
||||
// Rotate nodes[1:] by one position
|
||||
first := nodeList[1]
|
||||
copy(nodeList[1:], nodeList[2:])
|
||||
nodeList[len(nodeList)-1] = first
|
||||
}
|
||||
}
|
||||
|
||||
walker := NewNodeToNodeWalker(
|
||||
fmt.Sprintf("walker-%d", i),
|
||||
nodeList,
|
||||
speed,
|
||||
waitTime,
|
||||
)
|
||||
|
||||
// Start at first node position
|
||||
walker.Position = nodeList[0].Position
|
||||
walker.NodeIndex = 1 // Target is second node
|
||||
|
||||
ws.Add(walker)
|
||||
}
|
||||
|
||||
return ws
|
||||
}
|
||||
|
||||
// CreateNodeToNodeWalkersNoWait creates node-to-node walkers that don't wait at nodes
|
||||
func CreateNodeToNodeWalkersNoWait(count int, nodes *NodeSet, speed float64) *WalkerSet {
|
||||
return CreateNodeToNodeWalkers(count, nodes, speed, 0)
|
||||
}
|
||||
|
||||
// SimulationTick represents one tick of simulation state
|
||||
type SimulationTick struct {
|
||||
Timestamp time.Time `json:"timestamp"`
|
||||
|
|
|
|||
441
mothership/internal/simulator/walker_test.go
Normal file
441
mothership/internal/simulator/walker_test.go
Normal file
|
|
@ -0,0 +1,441 @@
|
|||
package simulator
|
||||
|
||||
import (
|
||||
"math"
|
||||
"testing"
|
||||
"time"
|
||||
)
|
||||
|
||||
func TestNewNodeToNodeWalker(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-3", "Node 3", Point{X: 2.5, Y: 5, Z: 2}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalker("walker-1", nodes, 1.0, 2.0)
|
||||
|
||||
if w.Type != WalkerTypeNodeToNode {
|
||||
t.Errorf("Expected type %s, got %s", WalkerTypeNodeToNode, w.Type)
|
||||
}
|
||||
|
||||
if len(w.Nodes) != 3 {
|
||||
t.Errorf("Expected 3 nodes, got %d", len(w.Nodes))
|
||||
}
|
||||
|
||||
if w.Speed != 1.0 {
|
||||
t.Errorf("Expected speed 1.0, got %f", w.Speed)
|
||||
}
|
||||
|
||||
if w.WaitTime != 2.0 {
|
||||
t.Errorf("Expected wait time 2.0, got %f", w.WaitTime)
|
||||
}
|
||||
|
||||
if !w.ShouldWait {
|
||||
t.Error("Expected ShouldWait to be true")
|
||||
}
|
||||
|
||||
// Should start at first node position
|
||||
if w.Position.X != 0 || w.Position.Y != 0 {
|
||||
t.Errorf("Expected starting position at node-1, got %v", w.Position)
|
||||
}
|
||||
|
||||
// Target should be second node
|
||||
if w.NodeIndex != 1 {
|
||||
t.Errorf("Expected NodeIndex 1, got %d", w.NodeIndex)
|
||||
}
|
||||
}
|
||||
|
||||
func TestNewNodeToNodeWalkerNoWait(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalkerNoWait("walker-1", nodes, 1.0)
|
||||
|
||||
if w.ShouldWait {
|
||||
t.Error("Expected ShouldWait to be false")
|
||||
}
|
||||
|
||||
if w.WaitTime != 0 {
|
||||
t.Errorf("Expected wait time 0, got %f", w.WaitTime)
|
||||
}
|
||||
}
|
||||
|
||||
func TestNewNodeToNodeWalkerPanicsOnEmptyNodes(t *testing.T) {
|
||||
defer func() {
|
||||
if r := recover(); r == nil {
|
||||
t.Error("Expected panic on empty nodes slice")
|
||||
}
|
||||
}()
|
||||
|
||||
_ = NewNodeToNodeWalker("walker-1", []*Node{}, 1.0, 0)
|
||||
}
|
||||
|
||||
func TestNewNodeToNodeWalkerPanicsOnSingleNode(t *testing.T) {
|
||||
defer func() {
|
||||
if r := recover(); r == nil {
|
||||
t.Error("Expected panic on single node")
|
||||
}
|
||||
}()
|
||||
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
}
|
||||
_ = NewNodeToNodeWalker("walker-1", nodes, 1.0, 0)
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerMovement(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 3, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalkerNoWait("walker-1", nodes, 1.0, 0)
|
||||
space := DefaultSpace()
|
||||
|
||||
// Starting position
|
||||
startX := w.Position.X
|
||||
|
||||
// Update for 1 second
|
||||
dt := 0.1
|
||||
for i := 0; i < 10; i++ {
|
||||
w.Update(dt, space)
|
||||
}
|
||||
|
||||
// Should have moved towards node-2
|
||||
if w.Position.X <= startX {
|
||||
t.Errorf("Expected walker to move towards node-2 (X increased), but X went from %f to %f", startX, w.Position.X)
|
||||
}
|
||||
|
||||
// Velocity should be set towards node-2
|
||||
if w.Velocity.X <= 0 {
|
||||
t.Errorf("Expected positive X velocity, got %f", w.Velocity.X)
|
||||
}
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerArrival(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 3, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-3", "Node 3", Point{X: 3, Y: 5, Z: 2}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalkerNoWait("walker-1", nodes, 5.0, 0)
|
||||
space := DefaultSpace()
|
||||
|
||||
// Update until walker reaches node-2
|
||||
for i := 0; i < 100; i++ {
|
||||
w.Update(0.1, space)
|
||||
|
||||
// Check if we've moved past node-2
|
||||
if w.Position.X >= 2.7 && w.NodeIndex == 1 {
|
||||
// Should have advanced to next node
|
||||
if w.NodeIndex != 2 {
|
||||
t.Logf("Still at node index 1, position: %v", w.Position)
|
||||
}
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
// Eventually should reach node-2 and target node-3
|
||||
if w.NodeIndex == 1 {
|
||||
// Force position near node-2 to trigger advancement
|
||||
w.Position.X = 2.95
|
||||
w.Update(0.1, space)
|
||||
if w.NodeIndex != 2 {
|
||||
t.Errorf("Expected NodeIndex to advance to 2 after reaching node-2, got %d", w.NodeIndex)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerWithWait(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 3, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
waitTime := 1.0 // 1 second wait
|
||||
w := NewNodeToNodeWalker("walker-1", nodes, 5.0, waitTime)
|
||||
space := DefaultSpace()
|
||||
|
||||
// Move walker to node-2
|
||||
w.Position.X = 2.95
|
||||
w.Position.Y = 0
|
||||
w.Position.Z = 1.7
|
||||
|
||||
// First update - should start waiting
|
||||
w.Update(0.1, space)
|
||||
|
||||
if w.WaitTimer <= 0 {
|
||||
t.Error("Expected WaitTimer to be positive after arrival")
|
||||
}
|
||||
|
||||
// Check velocity is zero while waiting
|
||||
if w.Velocity.X != 0 || w.Velocity.Y != 0 || w.Velocity.Z != 0 {
|
||||
t.Errorf("Expected zero velocity while waiting, got %v", w.Velocity)
|
||||
}
|
||||
|
||||
// Update until wait time expires
|
||||
updatesToExpire := int(waitTime/0.1) + 1
|
||||
for i := 0; i < updatesToExpire; i++ {
|
||||
w.Update(0.1, space)
|
||||
}
|
||||
|
||||
// Wait timer should have been reset for next node
|
||||
if w.WaitTimer != waitTime {
|
||||
t.Errorf("Expected WaitTimer to be reset to %f, got %f", waitTime, w.WaitTimer)
|
||||
}
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerFallsBackToRandomWalk(t *testing.T) {
|
||||
// Create walker with no nodes
|
||||
w := &Walker{
|
||||
ID: "walker-1",
|
||||
Type: WalkerTypeNodeToNode,
|
||||
Position: Point{X: 1, Y: 1, Z: 1.7},
|
||||
Speed: 1.0,
|
||||
Height: 1.7,
|
||||
Nodes: []*Node{},
|
||||
Velocity: Point{X: 0.1, Y: 0.1, Z: 0},
|
||||
}
|
||||
|
||||
space := DefaultSpace()
|
||||
initialX := w.Position.X
|
||||
|
||||
// Update should fall back to random walk
|
||||
w.Update(0.1, space)
|
||||
|
||||
// Position should have changed (random walk behavior)
|
||||
// but not in a deterministic direction
|
||||
if w.Position.X == initialX && w.Position.Y == 1 {
|
||||
t.Error("Expected position to change during random walk fallback")
|
||||
}
|
||||
}
|
||||
|
||||
func TestCreateNodeToNodeWalkers(t *testing.T) {
|
||||
nodes := NewNodeSet()
|
||||
nodes.AddVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2})
|
||||
nodes.AddVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2})
|
||||
nodes.AddVirtualNode("node-3", "Node 3", Point{X: 2.5, Y: 5, Z: 2})
|
||||
|
||||
ws := CreateNodeToNodeWalkers(3, nodes, 1.0, 2.0)
|
||||
|
||||
if ws.Count() != 3 {
|
||||
t.Errorf("Expected 3 walkers, got %d", ws.Count())
|
||||
}
|
||||
|
||||
for i, w := range ws.All() {
|
||||
if w.Type != WalkerTypeNodeToNode {
|
||||
t.Errorf("Walker %d: expected type %s, got %s", i, WalkerTypeNodeToNode, w.Type)
|
||||
}
|
||||
|
||||
if len(w.Nodes) != 3 {
|
||||
t.Errorf("Walker %d: expected 3 nodes, got %d", i, len(w.Nodes))
|
||||
}
|
||||
|
||||
if !w.ShouldWait {
|
||||
t.Errorf("Walker %d: expected ShouldWait to be true", i)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestCreateNodeToNodeWalkersNoWait(t *testing.T) {
|
||||
nodes := NewNodeSet()
|
||||
nodes.AddVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2})
|
||||
nodes.AddVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2})
|
||||
|
||||
ws := CreateNodeToNodeWalkersNoWait(2, nodes, 1.0)
|
||||
|
||||
if ws.Count() != 2 {
|
||||
t.Errorf("Expected 2 walkers, got %d", ws.Count())
|
||||
}
|
||||
|
||||
for _, w := range ws.All() {
|
||||
if w.ShouldWait {
|
||||
t.Error("Expected ShouldWait to be false for no-wait walkers")
|
||||
}
|
||||
|
||||
if w.WaitTime != 0 {
|
||||
t.Errorf("Expected WaitTime 0, got %f", w.WaitTime)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestCreateNodeToNodeWalkersWithEmptyNodes(t *testing.T) {
|
||||
nodes := NewNodeSet()
|
||||
ws := CreateNodeToNodeWalkers(3, nodes, 1.0, 0)
|
||||
|
||||
if ws.Count() != 0 {
|
||||
t.Errorf("Expected 0 walkers with empty node set, got %d", ws.Count())
|
||||
}
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerSpeedVariation(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 10, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalkerNoWait("walker-1", nodes, 1.0, 0)
|
||||
space := DefaultSpace()
|
||||
|
||||
// Collect velocities over multiple updates
|
||||
velocities := make([]float64, 0, 20)
|
||||
|
||||
for i := 0; i < 20; i++ {
|
||||
w.Update(0.1, space)
|
||||
|
||||
// Calculate speed from velocity
|
||||
speed := math.Sqrt(w.Velocity.X*w.Velocity.X + w.Velocity.Y*w.Velocity.Y + w.Velocity.Z*w.Velocity.Z)
|
||||
if speed > 0.01 {
|
||||
velocities = append(velocities, speed)
|
||||
}
|
||||
}
|
||||
|
||||
if len(velocities) == 0 {
|
||||
t.Fatal("Expected some non-zero velocities")
|
||||
}
|
||||
|
||||
// Check for variation (should not all be the same)
|
||||
minSpeed := velocities[0]
|
||||
maxSpeed := velocities[0]
|
||||
for _, v := range velocities {
|
||||
if v < minSpeed {
|
||||
minSpeed = v
|
||||
}
|
||||
if v > maxSpeed {
|
||||
maxSpeed = v
|
||||
}
|
||||
}
|
||||
|
||||
// Should have at least some variation (0.8x to 1.2x base speed)
|
||||
if maxSpeed-minSpeed < 0.1 {
|
||||
t.Errorf("Expected speed variation, but min=%f, max=%f", minSpeed, maxSpeed)
|
||||
}
|
||||
|
||||
// All speeds should be reasonable (not exceeding base speed significantly)
|
||||
for _, v := range velocities {
|
||||
if v > 1.5 {
|
||||
t.Errorf("Speed %f exceeds reasonable maximum", v)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerDecelerationNearTarget(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalkerNoWait("walker-1", nodes, 1.0, 0)
|
||||
space := DefaultSpace()
|
||||
|
||||
// Position walker close to target
|
||||
w.Position.X = 4.2 // About 0.8m from target
|
||||
|
||||
// Get speed before close approach
|
||||
w.Update(0.1, space)
|
||||
farSpeed := math.Sqrt(w.Velocity.X*w.Velocity.X + w.Velocity.Y*w.Velocity.Y)
|
||||
|
||||
// Position walker very close to target
|
||||
w.Position.X = 4.8 // About 0.2m from target
|
||||
w.Update(0.1, space)
|
||||
nearSpeed := math.Sqrt(w.Velocity.X*w.Velocity.X + w.Velocity.Y*w.Velocity.Y)
|
||||
|
||||
// Should decelerate when close
|
||||
if nearSpeed >= farSpeed {
|
||||
t.Errorf("Expected deceleration near target: far=%f, near=%f", farSpeed, nearSpeed)
|
||||
}
|
||||
}
|
||||
|
||||
func TestNodeToNodeWalkerMaintainsHeight(t *testing.T) {
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2.5}),
|
||||
}
|
||||
|
||||
w := NewNodeToNodeWalkerNoWait("walker-1", nodes, 1.0, 0)
|
||||
space := DefaultSpace()
|
||||
|
||||
expectedHeight := w.Height
|
||||
|
||||
// Update multiple times
|
||||
for i := 0; i < 10; i++ {
|
||||
w.Update(0.1, space)
|
||||
|
||||
if math.Abs(w.Position.Z-expectedHeight) > 0.01 {
|
||||
t.Errorf("Expected height %f, got %f", expectedHeight, w.Position.Z)
|
||||
}
|
||||
}
|
||||
}
|
||||
|
||||
func TestWalkerSetAddNodeToNodeWalker(t *testing.T) {
|
||||
ws := NewWalkerSet()
|
||||
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
ws.AddNodeToNodeWalker("walker-1", nodes, 1.0, 2.0)
|
||||
|
||||
if ws.Count() != 1 {
|
||||
t.Errorf("Expected 1 walker, got %d", ws.Count())
|
||||
}
|
||||
|
||||
walker := ws.All()[0]
|
||||
if walker.Type != WalkerTypeNodeToNode {
|
||||
t.Errorf("Expected type %s, got %s", WalkerTypeNodeToNode, walker.Type)
|
||||
}
|
||||
|
||||
if walker.WaitTime != 2.0 {
|
||||
t.Errorf("Expected wait time 2.0, got %f", walker.WaitTime)
|
||||
}
|
||||
}
|
||||
|
||||
func TestWalkerSetAddNodeToNodeWalkerNoWait(t *testing.T) {
|
||||
ws := NewWalkerSet()
|
||||
|
||||
nodes := []*Node{
|
||||
NewVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2}),
|
||||
NewVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2}),
|
||||
}
|
||||
|
||||
ws.AddNodeToNodeWalkerNoWait("walker-1", nodes, 1.0)
|
||||
|
||||
if ws.Count() != 1 {
|
||||
t.Errorf("Expected 1 walker, got %d", ws.Count())
|
||||
}
|
||||
|
||||
walker := ws.All()[0]
|
||||
if walker.ShouldWait {
|
||||
t.Error("Expected ShouldWait to be false")
|
||||
}
|
||||
}
|
||||
|
||||
func TestGenerateTicksWithNodeToNodeWalkers(t *testing.T) {
|
||||
nodes := NewNodeSet()
|
||||
nodes.AddVirtualNode("node-1", "Node 1", Point{X: 0, Y: 0, Z: 2})
|
||||
nodes.AddVirtualNode("node-2", "Node 2", Point{X: 5, Y: 0, Z: 2})
|
||||
|
||||
ws := CreateNodeToNodeWalkers(1, nodes, 1.0, 0)
|
||||
space := DefaultSpace()
|
||||
|
||||
// Generate ticks for 1 second at 10 Hz
|
||||
ticks := 0
|
||||
tickChan := ws.GenerateTicks(10, 1*time.Second, space)
|
||||
|
||||
for range tickChan {
|
||||
ticks++
|
||||
if ticks > 15 {
|
||||
t.Error("Too many ticks generated")
|
||||
break
|
||||
}
|
||||
}
|
||||
|
||||
if ticks < 8 {
|
||||
t.Errorf("Expected at least 8 ticks, got %d", ticks)
|
||||
}
|
||||
}
|
||||
Loading…
Add table
Reference in a new issue