a_star_dynamic.py

import heapq
import numpy as np
import matplotlib.pyplot as plt
from scipy.special import comb

class SmartAgriNavigator:
    def __init__(self, grid, start, goal):
        self.grid = grid
        self.start = start
        self.goal = goal
        self.rows, self.cols = grid.shape
        self.raw_path = []
        self.smooth_path = []
        
        # Préparation de la zone de sécurité (inflation des obstacles)
        self.safety_grid = self._inflate_obstacles(grid)

    def _inflate_obstacles(self, grid):
        """ Crée une zone tampon (0.5) autour des murs (1) """
        new_grid = np.copy(grid)
        for r in range(self.rows):
            for c in range(self.cols):
                if grid[r, c] == 1:
                    for dr, dc in [(0,1),(0,-1),(1,0),(-1,0)]:
                        nr, nc = r + dr, c + dc
                        if 0 <= nr < self.rows and 0 <= nc < self.cols:
                            if new_grid[nr, nc] == 0:
                                new_grid[nr, nc] = 0.5 
        return new_grid

    def heuristic(self, pos):
        """ Distance de Manhattan (Standard MDPI) """
        return abs(pos[0] - self.goal[0]) + abs(pos[1] - self.goal[1])

    def get_dynamic_k(self, pos):
        """ 
        Version simplifiée : 
        k=3.0 si on est dans la zone de sécurité (proche mur)
        k=1.0 si on est en zone libre
        """
        if self.safety_grid[pos] == 0.5:
            return 3.0
        return 1.0

    def get_neighbors(self, pos):
        neighbors = []
        for dr, dc in [(0, 1), (1, 0), (-1, 0), (0, -1)]:
            nr, nc = pos[0] + dr, pos[1] + dc
            if 0 <= nr < self.rows and 0 <= nc < self.cols:
                if self.grid[nr, nc] == 0: # Ne passe pas à travers les murs
                    neighbors.append((nr, nc))
        return neighbors

    def a_star_dynamic(self):
        # Priority Queue
        frontier = []
        heapq.heappush(frontier, (0, self.start))
        came_from = {self.start: None}
        g_score = {self.start: 0}

        while frontier:
            _, current = heapq.heappop(frontier)

            if current == self.goal:
                self._reconstruct_path(came_from)
                return True

            for neighbor in self.get_neighbors(current):
                # Coût de passage (g) : pénalité si zone de sécurité
                move_cost = 1 if self.safety_grid[neighbor] == 0 else 5
                tentative_g = g_score[current] + move_cost
                
                if neighbor not in g_score or tentative_g < g_score[neighbor]:
                    g_score[neighbor] = tentative_g
                    
                    # Application du k dynamique simplifié
                    k = self.get_dynamic_k(neighbor)
                    f_score = tentative_g + (k * self.heuristic(neighbor))
                    
                    heapq.heappush(frontier, (f_score, neighbor))
                    came_from[neighbor] = current
        return False

    def _reconstruct_path(self, came_from):
        curr = self.goal
        path = []
        while curr:
            path.append(curr)
            curr = came_from[curr]
        self.raw_path = path[::-1]

    def bernstein_poly(self, i, n, t):
        return comb(n, i) * (t**i) * (1 - t)**(n-i)

    def generate_bezier_curve(self, num_points=200):
        if len(self.raw_path) < 3:
            self.smooth_path = self.raw_path
            return

        points = np.array(self.raw_path)
        n = len(points) - 1
        t = np.linspace(0, 1, num_points)
        
        curve_x = np.zeros(num_points)
        curve_y = np.zeros(num_points)

        for i in range(n + 1):
            poly = self.bernstein_poly(i, n, t)
            curve_x += points[i, 0] * poly
            curve_y += points[i, 1] * poly

        self.smooth_path = list(zip(curve_x, curve_y))

    def visualize(self):
        plt.figure(figsize=(10, 10))
        plt.imshow(self.grid, cmap='Greys', origin='lower', extent=[0, self.cols, 0, self.rows])

        if self.raw_path:
            ry, rx = zip(*self.raw_path)
            plt.plot(np.array(rx)+0.5, np.array(ry)+0.5, 'r--', alpha=0.4, label='A* Brut (Manhattan)')

        if self.smooth_path:
            by, bx = zip(*self.smooth_path)
            plt.plot(np.array(bx)+0.5, np.array(by)+0.5, 'b-', linewidth=3, label='Trajectoire Bézier Optimisée')

        plt.scatter(self.start[1]+0.5, self.start[0]+0.5, c='g', s=200, label='Départ', marker='s')
        plt.scatter(self.goal[1]+0.5, self.goal[0]+0.5, c='r', s=200, label='Arrivée', marker='s')

        plt.title("Navigation Optimisée sans calculate_ec\nA* Adaptatif + Courbes de Bézier")
        plt.legend()
        plt.grid(True, alpha=0.2)
        plt.show()

if __name__ == "__main__":
    # Grille de test 30x30
    grid = np.zeros((30, 30))
    grid[10, 5:20] = 1   # Mur horizontal

    grid[5:15, 20] = 1   # Mur vertical

    grid[20, 10:25] = 1  # Autre mur  # Obstacle vertical
    
    nav = SmartAgriNavigator(grid, (2, 2), (25, 25))
    if nav.a_star_dynamic():
        nav.generate_bezier_curve()
        nav.visualize()