c0ae81a6b0
Some checks are pending
CI Benchmark - Fusion Loop Timing / Fusion Loop Timing Benchmark (push) Waiting to run
Ran gofmt across the entire mothership codebase to ensure consistent code formatting per Go standards. All tests pass after formatting.
606 lines
17 KiB
Go
606 lines
17 KiB
Go
// Package simulator provides pre-deployment simulation capabilities for Spaxel.
|
|
//
|
|
// This package allows users to:
|
|
// - Define their space in a 3D editor (or via API)
|
|
// - Place virtual nodes at candidate positions
|
|
// - Generate synthetic "walkers" that move through the space
|
|
// - Compute expected CSI using propagation models
|
|
// - Apply the same Fresnel zone localization algorithm as live mode
|
|
// - View GDOP overlay, accuracy estimates, and minimum node recommendations
|
|
package simulator
|
|
|
|
import (
|
|
"fmt"
|
|
"log"
|
|
"math"
|
|
"math/rand"
|
|
"sync"
|
|
"time"
|
|
)
|
|
|
|
// Engine is the pre-deployment simulator engine.
|
|
type Engine struct {
|
|
mu sync.RWMutex
|
|
space *Space
|
|
nodes *NodeSet
|
|
walkers []*SimWalker
|
|
grid *Grid
|
|
links []Link
|
|
publishedResults *SimulationResult
|
|
subscribers []chan *SimulationResult
|
|
propagation *PropagationModel
|
|
accuracy *AccuracyEstimator
|
|
recommendations *RecommendationEngine
|
|
}
|
|
|
|
// SimWalker represents a simulated person moving through the space.
|
|
type SimWalker struct {
|
|
ID string `json:"id"`
|
|
Type WalkerType `json:"type"`
|
|
Position Point `json:"position"`
|
|
Velocity Point `json:"velocity"`
|
|
Path []Point `json:"path,omitempty"` // for path walks
|
|
PathIndex int `json:"path_index,omitempty"` // current position in path
|
|
TargetZones []string `json:"target_zones,omitempty"` // for zone walks
|
|
TrueHistory []Point `json:"true_history,omitempty"` // ground truth positions
|
|
}
|
|
|
|
// Grid is the 3D spatial grid for Fresnel accumulation.
|
|
type Grid struct {
|
|
CellSize float64 `json:"cell_size"` // meters
|
|
OriginX float64 `json:"origin_x"` // meters
|
|
OriginY float64 `json:"origin_y"` // meters
|
|
OriginZ float64 `json:"origin_z"` // meters
|
|
WidthCells int `json:"width_cells"` // number of cells in X
|
|
DepthCells int `json:"depth_cells"` // number of cells in Y
|
|
HeightCells int `json:"height_cells"` // number of cells in Z
|
|
Data []float64 `json:"data"` // flattened 3D array [z][x][y]
|
|
}
|
|
|
|
// ZoneInfo contains Fresnel zone information for a grid cell.
|
|
type ZoneInfo struct {
|
|
CellIndex int `json:"cell_index"` // flattened index
|
|
Zone int `json:"zone"` // Fresnel zone number
|
|
Decay float64 `json:"decay"` // zone decay factor
|
|
}
|
|
|
|
// SimulationResult contains the results of a simulation run.
|
|
type SimulationResult struct {
|
|
Timestamp int64 `json:"timestamp_ms"`
|
|
Blobs []BlobResult `json:"blobs"`
|
|
CoverageScore float64 `json:"coverage_score"` // 0-100
|
|
GDOPMap []float64 `json:"gdop_map"` // flattened grid
|
|
GridDimensions []int `json:"grid_dimensions"` // [width_cells, depth_cells, height_cells]
|
|
Recommendations []Recommendation `json:"recommendations"`
|
|
Accuracy AccuracyReport `json:"accuracy"`
|
|
ShoppingList ShoppingList `json:"shopping_list"`
|
|
}
|
|
|
|
// BlobResult is a simulated detection result.
|
|
type BlobResult struct {
|
|
ID int `json:"id"`
|
|
Position Point `json:"position"`
|
|
Confidence float64 `json:"confidence"`
|
|
Velocity Point `json:"velocity"`
|
|
WalkerID string `json:"walker_id"`
|
|
TrueError float64 `json:"true_error_m,omitempty"` // distance from true position
|
|
}
|
|
|
|
// NewEngine creates a new simulator engine.
|
|
func NewEngine(space *Space) *Engine {
|
|
return &Engine{
|
|
space: space,
|
|
nodes: NewNodeSet(),
|
|
walkers: make([]*SimWalker, 0),
|
|
subscribers: make([]chan *SimulationResult, 0),
|
|
propagation: NewPropagationModel(space),
|
|
accuracy: NewAccuracyEstimator(),
|
|
recommendations: NewRecommendationEngine(),
|
|
}
|
|
}
|
|
|
|
// SetSpace updates the space definition.
|
|
func (e *Engine) SetSpace(space *Space) {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
e.space = space
|
|
e.propagation = NewPropagationModel(space)
|
|
e.grid = nil // Invalidate grid
|
|
}
|
|
|
|
// AddVirtualNode adds a virtual node at the specified position.
|
|
func (e *Engine) AddVirtualNode(node *Node) error {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
// Validate position is within space
|
|
minX, minY, minZ, maxX, maxY, maxZ := e.space.Bounds()
|
|
if node.Position.X < minX || node.Position.X > maxX {
|
|
return ErrNodeOutsideSpace
|
|
}
|
|
if node.Position.Y < minY || node.Position.Y > maxY {
|
|
return ErrNodeOutsideSpace
|
|
}
|
|
if node.Position.Z < minZ || node.Position.Z > maxZ {
|
|
return ErrNodeOutsideSpace
|
|
}
|
|
|
|
e.nodes.Add(node)
|
|
e.links = nil // Invalidate links
|
|
e.grid = nil // Invalidate grid
|
|
|
|
log.Printf("[SIM] Added virtual node %s at (%.2f, %.2f, %.2f)", node.ID, node.Position.X, node.Position.Y, node.Position.Z)
|
|
|
|
return nil
|
|
}
|
|
|
|
// RemoveVirtualNode removes a virtual node by ID.
|
|
func (e *Engine) RemoveVirtualNode(id string) {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
if e.nodes.Remove(id) {
|
|
e.links = nil
|
|
e.grid = nil
|
|
log.Printf("[SIM] Removed virtual node %s", id)
|
|
}
|
|
}
|
|
|
|
// AddWalker adds a simulated walker.
|
|
func (e *Engine) AddWalker(walker *SimWalker) {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
e.walkers = append(e.walkers, walker)
|
|
walker.TrueHistory = make([]Point, 0)
|
|
log.Printf("[SIM] Added walker %s", walker.ID)
|
|
}
|
|
|
|
// RemoveWalker removes a walker by ID.
|
|
func (e *Engine) RemoveWalker(id string) {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
for i, w := range e.walkers {
|
|
if w.ID == id {
|
|
e.walkers = append(e.walkers[:i], e.walkers[i+1:]...)
|
|
log.Printf("[SIM] Removed walker %s", id)
|
|
return
|
|
}
|
|
}
|
|
}
|
|
|
|
// GetVirtualNodes returns all virtual nodes.
|
|
func (e *Engine) GetVirtualNodes() []*Node {
|
|
e.mu.RLock()
|
|
defer e.mu.RUnlock()
|
|
|
|
return e.nodes.All()
|
|
}
|
|
|
|
// GetWalkers returns all walkers.
|
|
func (e *Engine) GetWalkers() []*SimWalker {
|
|
e.mu.RLock()
|
|
defer e.mu.RUnlock()
|
|
|
|
walkers := make([]*SimWalker, len(e.walkers))
|
|
copy(walkers, e.walkers)
|
|
return walkers
|
|
}
|
|
|
|
// RunSimulation runs one simulation tick and publishes results.
|
|
func (e *Engine) RunSimulation() *SimulationResult {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
// Update grid if needed
|
|
if e.grid == nil {
|
|
e.initializeGrid()
|
|
}
|
|
|
|
// Update walker positions
|
|
e.updateWalkers()
|
|
|
|
// Generate virtual links between all node pairs
|
|
if e.links == nil {
|
|
e.generateLinks()
|
|
}
|
|
|
|
// Compute CSI at each walker position
|
|
blobResults := e.detectBlobs()
|
|
|
|
// Compute GDOP map
|
|
gdopMap := e.computeGDOPMap()
|
|
|
|
// Compute coverage score
|
|
coverageScore := e.computeCoverageScore(gdopMap)
|
|
|
|
// Generate recommendations
|
|
recommendations := e.recommendations.Generate(e.space, e.nodes, gdopMap, coverageScore)
|
|
|
|
// Compute accuracy
|
|
accuracy := e.accuracy.Compute(e.walkers, blobResults)
|
|
|
|
// Generate shopping list
|
|
shoppingList := GenerateShoppingListFromResults(e.space, e.nodes, coverageScore, accuracy)
|
|
|
|
result := &SimulationResult{
|
|
Timestamp: time.Now().UnixMilli(),
|
|
Blobs: blobResults,
|
|
CoverageScore: coverageScore,
|
|
GDOPMap: gdopMap,
|
|
GridDimensions: []int{e.grid.WidthCells, e.grid.DepthCells, e.grid.HeightCells},
|
|
Recommendations: recommendations,
|
|
Accuracy: accuracy,
|
|
ShoppingList: shoppingList,
|
|
}
|
|
|
|
e.publishedResults = result
|
|
|
|
// Notify subscribers
|
|
for _, ch := range e.subscribers {
|
|
select {
|
|
case ch <- result:
|
|
default:
|
|
// Channel full, skip
|
|
}
|
|
}
|
|
|
|
return result
|
|
}
|
|
|
|
// Subscribe creates a channel for simulation result updates.
|
|
func (e *Engine) Subscribe() <-chan *SimulationResult {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
ch := make(chan *SimulationResult, 1)
|
|
e.subscribers = append(e.subscribers, ch)
|
|
return ch
|
|
}
|
|
|
|
// Unsubscribe removes a subscription channel.
|
|
func (e *Engine) Unsubscribe(ch <-chan *SimulationResult) {
|
|
e.mu.Lock()
|
|
defer e.mu.Unlock()
|
|
|
|
for i, sub := range e.subscribers {
|
|
if sub == ch {
|
|
e.subscribers = append(e.subscribers[:i], e.subscribers[i+1:]...)
|
|
close(sub)
|
|
return
|
|
}
|
|
}
|
|
}
|
|
|
|
// initializeGrid creates the spatial grid.
|
|
func (e *Engine) initializeGrid() {
|
|
const cellSize = 0.2 // 20cm cells
|
|
|
|
minX, minY, minZ, maxX, maxY, maxZ := e.space.Bounds()
|
|
|
|
widthCells := int(math.Ceil((maxX - minX) / cellSize))
|
|
depthCells := int(math.Ceil((maxY - minY) / cellSize))
|
|
heightCells := int(math.Ceil((maxZ - minZ) / cellSize))
|
|
|
|
e.grid = &Grid{
|
|
CellSize: cellSize,
|
|
OriginX: minX,
|
|
OriginY: minY,
|
|
OriginZ: minZ,
|
|
WidthCells: widthCells,
|
|
DepthCells: depthCells,
|
|
HeightCells: heightCells,
|
|
Data: make([]float64, widthCells*depthCells*heightCells),
|
|
}
|
|
|
|
log.Printf("[SIM] Grid initialized: %dx%dx%d cells", widthCells, depthCells, heightCells)
|
|
}
|
|
|
|
// generateLinks creates virtual links between all node pairs.
|
|
func (e *Engine) generateLinks() {
|
|
e.links = GenerateAllLinks(e.nodes)
|
|
log.Printf("[SIM] Generated %d links", len(e.links))
|
|
}
|
|
|
|
// updateWalkers updates all walker positions.
|
|
func (e *Engine) updateWalkers() {
|
|
const dt = 0.1 // 100ms time step
|
|
|
|
minX, minY, minZ, maxX, maxY, maxZ := e.space.Bounds()
|
|
|
|
for _, walker := range e.walkers {
|
|
// Record true position
|
|
walker.TrueHistory = append(walker.TrueHistory, walker.Position)
|
|
|
|
if walker.Type == WalkerTypePathFollow && len(walker.Path) > 0 {
|
|
// Follow path
|
|
target := walker.Path[walker.PathIndex]
|
|
dx := target.X - walker.Position.X
|
|
dy := target.Y - walker.Position.Y
|
|
dz := target.Z - walker.Position.Z
|
|
dist := math.Sqrt(dx*dx + dy*dy + dz*dz)
|
|
|
|
stepSize := 0.5 / float64(10) // 0.5 m/s at 10 Hz
|
|
|
|
if dist <= stepSize {
|
|
// Reached waypoint, move to next
|
|
walker.Position = target
|
|
walker.PathIndex = (walker.PathIndex + 1) % len(walker.Path)
|
|
} else {
|
|
// Move toward target
|
|
walker.Position.X += (dx / dist) * stepSize
|
|
walker.Position.Y += (dy / dist) * stepSize
|
|
walker.Position.Z += (dz / dist) * stepSize
|
|
}
|
|
} else {
|
|
// Random walk
|
|
walker.Position.X += walker.Velocity.X * dt
|
|
walker.Position.Y += walker.Velocity.Y * dt
|
|
walker.Position.Z += walker.Velocity.Z * dt
|
|
|
|
// Bounce off walls
|
|
if walker.Position.X < minX {
|
|
walker.Position.X = minX
|
|
walker.Velocity.X *= -1
|
|
}
|
|
if walker.Position.X > maxX {
|
|
walker.Position.X = maxX
|
|
walker.Velocity.X *= -1
|
|
}
|
|
if walker.Position.Y < minY {
|
|
walker.Position.Y = minY
|
|
walker.Velocity.Y *= -1
|
|
}
|
|
if walker.Position.Y > maxY {
|
|
walker.Position.Y = maxY
|
|
walker.Velocity.Y *= -1
|
|
}
|
|
if walker.Position.Z < minZ {
|
|
walker.Position.Z = minZ
|
|
walker.Velocity.Z *= -1
|
|
}
|
|
if walker.Position.Z > maxZ {
|
|
walker.Position.Z = maxZ
|
|
walker.Velocity.Z *= -1
|
|
}
|
|
|
|
// Random velocity perturbation
|
|
walker.Velocity.X += (rand.Float64() - 0.5) * 0.1
|
|
walker.Velocity.Y += (rand.Float64() - 0.5) * 0.1
|
|
walker.Velocity.Z += (rand.Float64() - 0.5) * 0.05
|
|
|
|
// Clamp velocity
|
|
maxSpeed := 0.5
|
|
speed := math.Sqrt(walker.Velocity.X*walker.Velocity.X + walker.Velocity.Y*walker.Velocity.Y)
|
|
if speed > maxSpeed {
|
|
scale := maxSpeed / speed
|
|
walker.Velocity.X *= scale
|
|
walker.Velocity.Y *= scale
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// detectBlobs runs the Fresnel zone accumulation to detect walker positions.
|
|
func (e *Engine) detectBlobs() []BlobResult {
|
|
// Clear grid
|
|
for i := range e.grid.Data {
|
|
e.grid.Data[i] = 0
|
|
}
|
|
|
|
// Accumulate for each link and walker
|
|
for _, link := range e.links {
|
|
for _, walker := range e.walkers {
|
|
// Compute CSI amplitude at walker position
|
|
amplitude := e.propagation.AmplitudeAt(link.TX.Position, link.RX.Position, walker.Position)
|
|
|
|
// Add to grid cells covered by this link
|
|
for x := 0; x < e.grid.WidthCells; x++ {
|
|
for y := 0; y < e.grid.DepthCells; y++ {
|
|
for z := 0; z < e.grid.HeightCells; z++ {
|
|
// Cell center position
|
|
cx := e.grid.OriginX + float64(x)*e.grid.CellSize + e.grid.CellSize/2
|
|
cy := e.grid.OriginY + float64(y)*e.grid.CellSize + e.grid.CellSize/2
|
|
cz := e.grid.OriginZ + float64(z)*e.grid.CellSize + e.grid.CellSize/2
|
|
cellPos := Point{X: cx, Y: cy, Z: cz}
|
|
|
|
// Check if in Fresnel zone
|
|
zone := FresnelZoneNumber(link.TX.Position, link.RX.Position, cellPos)
|
|
if zone > 5 {
|
|
continue
|
|
}
|
|
|
|
// Zone decay (default decay_rate = 2.0)
|
|
decay := 1.0 / math.Pow(float64(zone), 2.0)
|
|
|
|
cellIndex := z*e.grid.WidthCells*e.grid.DepthCells + x*e.grid.DepthCells + y
|
|
contribution := amplitude * decay
|
|
e.grid.Data[cellIndex] += contribution
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
// Extract peaks (blobs)
|
|
blobs := make([]BlobResult, 0)
|
|
blobID := 1
|
|
|
|
for z := 0; z < e.grid.HeightCells; z++ {
|
|
for x := 0; x < e.grid.WidthCells; x++ {
|
|
for y := 0; y < e.grid.DepthCells; y++ {
|
|
cellIndex := z*e.grid.WidthCells*e.grid.DepthCells + x*e.grid.DepthCells + y
|
|
value := e.grid.Data[cellIndex]
|
|
|
|
if value < 0.1 {
|
|
continue // Below threshold
|
|
}
|
|
|
|
// Check if this is a local maximum
|
|
if e.isLocalMaximum(x, y, z, value) {
|
|
// Compute position from cell index
|
|
posX := e.grid.OriginX + float64(x)*e.grid.CellSize + e.grid.CellSize/2
|
|
posY := e.grid.OriginY + float64(y)*e.grid.CellSize + e.grid.CellSize/2
|
|
posZ := e.grid.OriginZ + float64(z)*e.grid.CellSize + e.grid.CellSize/2
|
|
blobPos := Point{X: posX, Y: posY, Z: posZ}
|
|
|
|
// Find nearest walker and compute true error
|
|
nearestWalker := ""
|
|
minDist := 9999.0
|
|
for _, walker := range e.walkers {
|
|
dist := blobPos.Distance(walker.Position)
|
|
if dist < minDist {
|
|
minDist = dist
|
|
nearestWalker = walker.ID
|
|
}
|
|
}
|
|
|
|
blobs = append(blobs, BlobResult{
|
|
ID: blobID,
|
|
Position: blobPos,
|
|
Confidence: math.Min(1.0, value/5.0), // Normalize confidence
|
|
WalkerID: nearestWalker,
|
|
TrueError: minDist,
|
|
})
|
|
blobID++
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return blobs
|
|
}
|
|
|
|
// isLocalMaximum checks if a cell is a local maximum in its 6-neighborhood.
|
|
func (e *Engine) isLocalMaximum(x, y, z int, value float64) bool {
|
|
// Check 6-connected neighbors
|
|
neighbors := [][3]int{
|
|
{x - 1, y, z}, {x + 1, y, z},
|
|
{x, y - 1, z}, {x, y + 1, z},
|
|
{x, y, z - 1}, {x, y, z + 1},
|
|
}
|
|
|
|
for _, n := range neighbors {
|
|
if n[0] < 0 || n[0] >= e.grid.WidthCells ||
|
|
n[1] < 0 || n[1] >= e.grid.DepthCells ||
|
|
n[2] < 0 || n[2] >= e.grid.HeightCells {
|
|
continue
|
|
}
|
|
|
|
idx := n[2]*e.grid.WidthCells*e.grid.DepthCells + n[0]*e.grid.DepthCells + n[1]
|
|
if e.grid.Data[idx] > value {
|
|
return false
|
|
}
|
|
}
|
|
|
|
return true
|
|
}
|
|
|
|
// computeGDOPMap computes GDOP values for each grid cell.
|
|
func (e *Engine) computeGDOPMap() []float64 {
|
|
gdopMap := make([]float64, len(e.grid.Data))
|
|
|
|
if len(e.links) < 2 {
|
|
// Not enough links, set all to infinity
|
|
for i := range gdopMap {
|
|
gdopMap[i] = 9999.0
|
|
}
|
|
return gdopMap
|
|
}
|
|
|
|
for z := 0; z < e.grid.HeightCells; z++ {
|
|
for x := 0; x < e.grid.WidthCells; x++ {
|
|
for y := 0; y < e.grid.DepthCells; y++ {
|
|
cellIndex := z*e.grid.WidthCells*e.grid.DepthCells + x*e.grid.DepthCells + y
|
|
|
|
// Cell position
|
|
cx := e.grid.OriginX + float64(x)*e.grid.CellSize + e.grid.CellSize/2
|
|
cy := e.grid.OriginY + float64(y)*e.grid.CellSize + e.grid.CellSize/2
|
|
cz := e.grid.OriginZ + float64(z)*e.grid.CellSize + e.grid.CellSize/2
|
|
|
|
gdopMap[cellIndex] = e.computeGDOPAt(cx, cy, cz)
|
|
}
|
|
}
|
|
}
|
|
|
|
return gdopMap
|
|
}
|
|
|
|
// computeGDOPAt computes GDOP at a specific position.
|
|
func (e *Engine) computeGDOPAt(x, y, z float64) float64 {
|
|
point := Point{X: x, Y: y, Z: z}
|
|
|
|
// Collect links that cover this point (within zone 5)
|
|
var angles []float64
|
|
linkCount := 0
|
|
|
|
for _, link := range e.links {
|
|
// Check if this point is within zone 5
|
|
d1 := point.Distance(link.TX.Position)
|
|
d2 := point.Distance(link.RX.Position)
|
|
totalDist := d1 + d2
|
|
deltaL := totalDist - link.TX.Position.Distance(link.RX.Position)
|
|
|
|
zoneNumber := int(math.Ceil(deltaL / HalfWavelength))
|
|
|
|
if zoneNumber <= 5 {
|
|
linkCount++
|
|
// Compute angle to link direction
|
|
angle := math.Atan2(link.RX.Position.Y-link.TX.Position.Y, link.RX.Position.X-link.TX.Position.X)
|
|
angles = append(angles, angle)
|
|
}
|
|
}
|
|
|
|
if linkCount < 2 {
|
|
return 9999.0 // Infinity
|
|
}
|
|
|
|
// Build Fisher information matrix
|
|
var sumCos2, sumSin2, sumSinCos float64
|
|
for _, angle := range angles {
|
|
sumCos2 += math.Cos(angle) * math.Cos(angle)
|
|
sumSin2 += math.Sin(angle) * math.Sin(angle)
|
|
sumSinCos += math.Sin(angle) * math.Cos(angle)
|
|
}
|
|
|
|
detF := sumCos2*sumSin2 - sumSinCos*sumSinCos
|
|
if detF < 1e-6 {
|
|
return 9999.0 // Collinear links
|
|
}
|
|
|
|
// GDOP = sqrt(trace(F^-1)) = sqrt((sumCos2 + sumSin2) / detF)
|
|
traceInv := (sumCos2 + sumSin2) / detF
|
|
gdop := math.Sqrt(traceInv)
|
|
|
|
return gdop
|
|
}
|
|
|
|
// computeCoverageScore calculates the percentage of cells with "good" GDOP.
|
|
func (e *Engine) computeCoverageScore(gdopMap []float64) float64 {
|
|
if len(gdopMap) == 0 {
|
|
return 0
|
|
}
|
|
|
|
goodCells := 0
|
|
for _, gdop := range gdopMap {
|
|
if gdop < 4.0 { // Good or excellent GDOP
|
|
goodCells++
|
|
}
|
|
}
|
|
|
|
return 100.0 * float64(goodCells) / float64(len(gdopMap))
|
|
}
|
|
|
|
// GetResults returns the most recent simulation results from the engine.
|
|
func (e *Engine) GetResults() *SimulationResult {
|
|
e.mu.RLock()
|
|
defer e.mu.RUnlock()
|
|
return e.publishedResults
|
|
}
|
|
|
|
// Errors
|
|
var (
|
|
ErrNodeOutsideSpace = fmt.Errorf("node position is outside the defined space")
|
|
)
|