How to Add New Types of Obstacles to the Peng Simulation Environment

You can add new obstacle types to Peng by defining a common ObstacleTrait interface, implementing it for your custom geometry, and updating the Maze container to store trait objects instead of concrete spherical obstacles.

The Peng simulation environment, maintained in the makeecat/peng repository, provides a lightweight Rust-based framework for UAV trajectory planning. While the default implementation only supports spherical obstacles defined in src/lib.rs lines 63-78, the modular architecture allows you to extend the system with complex geometries like boxes, cylinders, or dynamic moving walls without rewriting the core planning algorithms.

Understanding the Obstacle Architecture in Peng

Before extending the system, you must understand how Peng currently handles obstacle avoidance. The architecture separates obstacle data storage from planning logic through three key components.

The Obstacle Struct

The default spherical obstacle is defined in src/lib.rs lines 63-78 as a simple struct:

pub struct Obstacle {
    pub position: Vector3<f32>,
    pub velocity: Vector3<f32>,
    pub radius: f32,
}

This struct stores the position, velocity, and radius of a spherical obstacle. The simulation uses these fields to calculate repulsive forces in the obstacle avoidance planner.

The Maze Container

The simulation-wide list of obstacles lives in the Maze struct defined in src/lib.rs lines 19-25:

pub struct Maze {
    pub bounds: Vector3<f32>,
    pub obstacles: Vec<Obstacle>,  // Currently hardcoded to spheres
    pub rng: StdRng,
}

The Maze acts as the central registry that generates and stores all obstacles. When you add new obstacle types, you must modify this container to support heterogeneous collections.

The Planner Integration

The PlannerManager bridges the Maze and the obstacle avoidance algorithm. In src/lib.rs lines 13-23, the update method passes the obstacle slice to the planner:

pub fn update(&mut self, obstacles: &[Obstacle], dt: f32) -> Vector3<f32> {
    // Delegates to ObstacleAvoidancePlanner
    self.planner.plan(obstacles, self.current_position, self.goal, dt)
}

The ObstacleAvoidancePlanner (lines 94-100) consumes this list to compute repulsive forces using the spherical distance formula. To support new shapes, you must abstract this interaction through a trait interface.

Step-by-Step Guide to Adding Custom Obstacle Types

To introduce boxes, cylinders, or dynamic hazards, you will refactor the obstacle system to use trait objects instead of concrete structs. This approach maintains backward compatibility while enabling polymorphic behavior.

Step 1: Define the ObstacleTrait Interface

Create a common trait in src/lib.rs that abstracts the required obstacle properties. This interface must provide position, velocity, and signed distance calculations:

pub trait ObstacleTrait: Send + Sync {
    fn position(&self) -> Vector3<f32>;
    fn velocity(&self) -> Vector3<f32>;
    fn distance_to(&self, point: Vector3<f32>) -> f32; // Signed distance
}

The Send + Sync bounds ensure thread safety if you later parallelize the simulation. The distance_to method returns the signed distance from any point in space to the obstacle surface, which generalizes the spherical radius check to arbitrary geometries.

Step 2: Implement the Trait for Existing Spheres

Maintain backward compatibility by implementing ObstacleTrait for the original Obstacle struct:

impl ObstacleTrait for Obstacle {
    fn position(&self) -> Vector3<f32> {
        self.position
    }
    
    fn velocity(&self) -> Vector3<f32> {
        self.velocity
    }
    
    fn distance_to(&self, point: Vector3<f32>) -> f32 {
        (point - self.position).norm() - self.radius
    }
}

This implementation preserves the existing behavior while adapting the struct to the new interface.

Step 3: Create New Obstacle Structs

Define a new struct for your custom geometry. Here is an example implementation for a box-shaped obstacle with configurable orientation:

#[derive(Clone)]
pub struct BoxObstacle {
    pub center: Vector3<f32>,
    pub half_extents: Vector3<f32>, // Half-size along each axis
    pub orientation: UnitQuaternion<f32>,
    pub velocity: Vector3<f32>,
}

impl ObstacleTrait for BoxObstacle {
    fn position(&self) -> Vector3<f32> {
        self.center
    }
    
    fn velocity(&self) -> Vector3<f32> {
        self.velocity
    }
    
    fn distance_to(&self, point: Vector3<f32>) -> f32 {
        // Transform point into box's local coordinate frame
        let local = self.orientation.inverse() * (point - self.center);
        
        // Compute signed distance to axis-aligned box
        let q = local.abs() - self.half_extents;
        let outside_dist = q.map(|v| v.max(0.0)).norm();
        let inside_dist = q.iter().fold(0.0, |acc, &v| acc.min(v).min(0.0));
        
        outside_dist + inside_dist
    }
}

This implementation uses a signed distance function (SDF) that handles both points outside and inside the box volume. You can apply the same pattern to cylinders, capsules, or convex polyhedra by implementing their respective SDFs.

Step 4: Update the Maze Container to Use Trait Objects

Modify the Maze struct to store a heterogeneous collection of obstacles. Replace the concrete Vec<Obstacle> with a vector of trait objects:

pub struct Maze {
    pub bounds: Vector3<f32>,
    pub obstacles: Vec<Box<dyn ObstacleTrait>>, // Now supports mixed types
    pub rng: StdRng,
}

When generating obstacles, wrap concrete instances in Box::new():

// Example: Mixed obstacle generation
self.obstacles = (0..num_obstacles)
    .map(|_| {
        let pos = random_position(&mut self.rng, self.bounds);
        let vel = random_velocity(&mut self.rng);
        
        if self.rng.gen_bool(0.5) {
            // Spherical obstacle
            Box::new(Obstacle::new(pos, vel, 0.5)) as Box<dyn ObstacleTrait>
        } else {
            // Box obstacle
            Box::new(BoxObstacle {
                center: pos,
                half_extents: Vector3::new(0.3, 0.3, 0.1),
                orientation: UnitQuaternion::identity(),
                velocity: vel,
            }) as Box<dyn ObstacleTrait>
        }
    })
    .collect();

Step 5: Modify the Obstacle Avoidance Planner

Update the ObstacleAvoidancePlanner to use the trait methods instead of direct field access. The repulsive force calculation now calls distance_to and position polymorphically:

impl ObstacleAvoidancePlanner {
    pub fn plan(
        &self,
        obstacles: &[Box<dyn ObstacleTrait>],
        current_pos: Vector3<f32>,
        goal: Vector3<f32>,
        dt: f32,
    ) -> Vector3<f32> {
        let mut f_rep = Vector3::zeros();
        
        for obstacle in obstacles {
            let distance = obstacle.distance_to(current_pos);
            
            if distance < self.d0 {
                let diff = current_pos - obstacle.position();
                let dir = diff.normalize();
                
                // Repulsive force using signed distance
                f_rep += self.k_rep 
                    * (1.0 / distance - 1.0 / self.d0) 
                    * (1.0 / distance.powi(2)) 
                    * dir;
            }
        }
        
        // Combine with attractive force to goal...
        f_rep
    }
}

This refactoring allows the planner to handle spheres, boxes, and any future obstacle types uniformly, using the signed distance function to determine repulsion strength.

Configuration and Testing Considerations

After implementing the trait-based architecture, you must update the configuration system and verify behavior through testing.

Extending YAML Configuration in src/config.rs

If your simulation loads obstacle definitions from YAML or JSON files, extend the parser in src/config.rs to recognize the new types. Add a type field discriminator:

// Example configuration parsing
fn parse_obstacle(config: &serde_yaml::Value) -> Box<dyn ObstacleTrait> {
    let obs_type = config["type"].as_str().unwrap_or("sphere");
    let pos = parse_vector3(&config["position"]);
    let vel = parse_vector3(&config["velocity"]);
    
    match obs_type {
        "sphere" => {
            let radius = config["radius"].as_f64().unwrap_or(0.5) as f32;
            Box::new(Obstacle::new(pos, vel, radius))
        },
        "box" => {
            let half_extents = parse_vector3(&config["half_extents"]);
            let orientation = parse_quaternion(&config["orientation"]);
            Box::new(BoxObstacle {
                center: pos,
                half_extents,
                orientation,
                velocity: vel,
            })
        },
        _ => panic!("Unknown obstacle type: {}", obs_type),
    }
}

Unit Testing New Obstacle Types

Add comprehensive tests to ensure your new obstacles integrate correctly with the planning pipeline:

#[cfg(test)]
mod tests {
    use super::*;
    
    #[test]
    fn test_box_obstacle_distance() {
        let box_obs = BoxObstacle {
            center: Vector3::new(0.0, 0.0, 0.0),
            half_extents: Vector3::new(1.0, 1.0, 1.0),
            orientation: UnitQuaternion::identity(),
            velocity: Vector3::zeros(),
        };
        
        // Point inside
        assert!(box_obs.distance_to(Vector3::new(0.0, 0.0, 0.0)) < 0.0);
        
        // Point outside
        let dist = box_obs.distance_to(Vector3::new(2.0, 0.0, 0.0));
        assert!((dist - 1.0).abs() < 1e-6); // Should be 1.0 unit from surface
    }
    
    #[test]
    fn test_mixed_obstacles_in_planner() {
        let sphere = Box::new(Obstacle::new(
            Vector3::new(1.0, 0.0, 0.0),
            Vector3::zeros(),
            0.5
        )) as Box<dyn ObstacleTrait>;
        
        let box_obs = Box::new(BoxObstacle {
            center: Vector3::new(-1.0, 0.0, 0.0),
            half_extents: Vector3::new(0.3, 0.3, 0.3),
            orientation: UnitQuaternion::identity(),
            velocity: Vector3::zeros(),
        }) as Box<dyn ObstacleTrait>;
        
        let obstacles: Vec<Box<dyn ObstacleTrait>> = vec![sphere, box_obs];
        
        // Verify planner produces valid output
        let planner = ObstacleAvoidancePlanner::new(1.0, 5.0);
        let velocity = planner.plan(&obstacles, Vector3::zeros(), Vector3::new(5.0, 0.0, 0.0), 0.1);
        
        assert!(!velocity.x.is_nan());
        assert!(!velocity.y.is_nan());
        assert!(!velocity.z.is_nan());
    }
}

These tests verify that your signed distance functions calculate correctly and that the PlannerManager can handle heterogeneous obstacle collections without runtime errors.

Summary

To add new types of obstacles to the Peng simulation environment, you must refactor the obstacle system to use trait objects rather than concrete structs. The key steps include:

• Defining an ObstacleTrait interface with position(), velocity(), and distance_to() methods in src/lib.rs
• Implementing the trait for the existing spherical Obstacle struct to maintain backward compatibility
• Creating new obstacle structs (such as BoxObstacle) that implement the trait using signed distance functions
• Updating the Maze struct to store Vec<Box<dyn ObstacleTrait>> instead of Vec<Obstacle>
• Modifying the ObstacleAvoidancePlanner to call trait methods polymorphically when calculating repulsive forces
• Extending src/config.rs to parse new obstacle types from configuration files if applicable

This architecture allows you to mix spheres, boxes, cylinders, and custom geometries in the same simulation while keeping the planning algorithms generic and maintainable.

Frequently Asked Questions

What is the Peng simulation environment?

Peng is an open-source Rust-based simulation framework for UAV trajectory planning and obstacle avoidance. It provides a lightweight architecture where obstacles, planners, and configuration management are separated into modular components, allowing researchers and developers to extend the environment with custom behaviors and geometries.

Why use trait objects instead of enums for obstacles?

Trait objects (Box<dyn ObstacleTrait>) allow you to define new obstacle types in separate modules or crates without modifying the core simulation code. Unlike enums, which require you to list all variants in a single type definition, trait objects enable open-ended extension and polymorphic behavior, making it easier to maintain backward compatibility when adding complex geometries like oriented bounding boxes or non-convex shapes.

How do I calculate signed distance for complex shapes?

For axis-aligned boxes, transform the query point into the box's local frame and compute the distance to the closest face using component-wise operations. For cylinders, calculate the distance in the radial direction and along the axis separately, then combine them. For arbitrary meshes, you may need to use a precomputed signed distance field (SDF) texture or a convex decomposition approach. The key requirement is that your distance_to method returns negative values inside the obstacle and positive values outside.

Will adding new obstacle types affect simulation performance?

Switching from concrete structs to trait objects introduces a small runtime overhead due to dynamic dispatch (v-table lookups), typically adding 1-2 nanoseconds per obstacle query. For simulations with fewer than 10,000 obstacles, this cost is negligible compared to the physics calculations. If performance becomes critical, you can use enum dispatch instead of trait objects, though this requires modifying the core obstacle enum definition whenever you add a new type.

{
  "mcpServers": {
    "instagit": {
      "command": "npx",
      "args": ["-y", "instagit@latest"]
    }
  }
}