mirror of
https://github.com/DM67-Developer/opensim-ai-npc-framework.git
synced 2026-08-14 00:47:59 +00:00
957 lines
36 KiB
Plaintext
957 lines
36 KiB
Plaintext
// Pathfinding.lsl — FINAL FIX: Corrected Wall vs Furniture Classification
|
|
// Fixes "Wooden Wall" being classified as furniture instead of wall
|
|
|
|
// ---- Opcodes (match Actions.lsl and other modules)
|
|
integer LM_INIT = 1;
|
|
integer LM_TIMER_CONFIG = 251; // Timer configuration updat
|
|
integer LM_CONFIG = 300;
|
|
|
|
// ---- Pathfinding opcodes
|
|
integer LM_PATH_REQUEST = 500; // Request pathfinding from start to end
|
|
integer LM_PATH_RESULT = 501; // Send pathfinding result back
|
|
integer LM_PATH_WAYPOINT = 502; // Individual waypoint data
|
|
integer LM_PATH_COMPLETE = 503; // Path following complete
|
|
integer LM_PATH_FAILED = 504; // Pathfinding failed
|
|
|
|
// ---- Senses integration
|
|
integer LM_SENSES_UPDATE = 401; // Receive obstacle data from Senses.lsl
|
|
|
|
// ---- Configuration defaults (overridden by general.cfg)
|
|
integer PATHFINDING_ENABLED = TRUE;
|
|
float GRID_SIZE = 2.0; // Increased for better navigation
|
|
integer MAX_SEARCH_NODES = 500; // Increased search complexity for walls
|
|
float OBSTACLE_AVOIDANCE_MARGIN = 0.8; // Reduced for tighter navigation
|
|
integer MAX_PATH_WAYPOINTS = 40; // More waypoints for complex paths
|
|
float PATHFINDING_TIMEOUT = 10.0; // Increased timeout for complex navigation
|
|
integer DEBUG_PATHFINDING = FALSE; // Debug mode
|
|
|
|
// ---- FIXED: Enhanced Wall Detection Configuration
|
|
float WALL_SIZE_THRESHOLD = 4.0; // Objects larger than this are walls
|
|
float WALL_HEIGHT_THRESHOLD = 3.0; // Objects taller than this are walls
|
|
integer ENABLE_SIZE_BASED_WALLS = TRUE; // Enable size-based wall classification
|
|
integer ENABLE_FURNITURE_DETECTION = TRUE; // Smart furniture detection
|
|
float WALL_DETECTION_THRESHOLD = 2.0; // How close before we consider hitting a wall
|
|
float PORTAL_SEARCH_RADIUS = 10.0; // How far to search for openings
|
|
float PORTAL_MIN_WIDTH = 1.5; // Minimum portal width for NPC passage
|
|
integer ENABLE_WALL_FOLLOWING = TRUE; // Enable wall-following behavior
|
|
integer ENABLE_PORTAL_DETECTION = TRUE; // Enable automatic portal discovery
|
|
float CORNER_ESCAPE_DISTANCE = 3.0; // How far to back up from corners
|
|
integer MAX_WALL_FOLLOW_ATTEMPTS = 8; // Prevent infinite wall-following
|
|
|
|
// ---- Runtime variables
|
|
key NPC = NULL_KEY;
|
|
list obstacle_positions = []; // [pos1, pos2, pos3, ...] - obstacles from Senses
|
|
list obstacle_sizes = []; // [size1, size2, size3, ...] - obstacle bounding sizes
|
|
list obstacle_details = []; // [type1, type2, type3, ...] - obstacle classification
|
|
list wall_segments = []; // Detected wall segments for navigation
|
|
list furniture_positions = []; // Furniture that can be navigated around
|
|
list portal_locations = []; // Discovered portal/opening locations
|
|
integer pathfinding_busy = FALSE;
|
|
float pathfinding_start_time = 0.0;
|
|
|
|
// ---- Config loading
|
|
string CONFIG_CARD = "general.cfg";
|
|
|
|
string get_card(string name) {
|
|
if (llGetInventoryType(name) != INVENTORY_NOTECARD) return "";
|
|
return osGetNotecard(name);
|
|
}
|
|
|
|
// ---- Safe math functions
|
|
float safeDivide(float numerator, float denominator) {
|
|
if (llFabs(denominator) < 0.001) return 0.0;
|
|
return numerator / denominator;
|
|
}
|
|
|
|
vector safeVecNorm(vector v) {
|
|
float magnitude = llVecMag(v);
|
|
if (magnitude < 0.001) return <1,0,0>;
|
|
return v / magnitude;
|
|
}
|
|
|
|
// ---- FIXED: Corrected Wall vs Furniture Classification
|
|
|
|
// PRIORITY CHECK: Check for walls FIRST (before furniture check)
|
|
integer isObstacleWall(vector pos, float size, string name, string desc, string objType) {
|
|
string searchText = llToLower(name + " " + desc);
|
|
|
|
// Method 1: EXPLICIT WALL KEYWORDS (checked FIRST)
|
|
if (llSubStringIndex(searchText, "wall") >= 0 ||
|
|
llSubStringIndex(searchText, "fence") >= 0 ||
|
|
llSubStringIndex(searchText, "barrier") >= 0 ||
|
|
llSubStringIndex(searchText, "building") >= 0 ||
|
|
llSubStringIndex(searchText, "structure") >= 0 ||
|
|
llSubStringIndex(searchText, "house") >= 0) {
|
|
return TRUE; // Definitely a wall - return immediately
|
|
}
|
|
|
|
// Method 2: Size-based detection (for unnamed walls)
|
|
if (ENABLE_SIZE_BASED_WALLS && size >= WALL_SIZE_THRESHOLD) {
|
|
return TRUE;
|
|
}
|
|
|
|
// Method 3: Large passive objects
|
|
if (objType == "passive" && size >= 5.0) {
|
|
return TRUE;
|
|
}
|
|
|
|
return FALSE; // Not a wall
|
|
}
|
|
|
|
// Check if object is furniture (ONLY if not already identified as wall)
|
|
integer isFurniture(string name, string desc, string objType, float size) {
|
|
string searchText = llToLower(name + " " + desc);
|
|
|
|
// CRITICAL: If it contains "wall", it's NOT furniture (should have been caught above)
|
|
if (llSubStringIndex(searchText, "wall") >= 0 ||
|
|
llSubStringIndex(searchText, "fence") >= 0 ||
|
|
llSubStringIndex(searchText, "barrier") >= 0 ||
|
|
llSubStringIndex(searchText, "building") >= 0 ||
|
|
llSubStringIndex(searchText, "structure") >= 0) {
|
|
return FALSE; // Definitely NOT furniture
|
|
}
|
|
|
|
// Explicit furniture keywords (only checked if not a wall)
|
|
if (llSubStringIndex(searchText, "table") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "chair") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "lamp") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "pillow") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "ottoman") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "seatable") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "couch") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "sofa") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "bed") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "desk") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "shelf") >= 0) return TRUE;
|
|
if (llSubStringIndex(searchText, "cabinet") >= 0) return TRUE;
|
|
|
|
// Object type classification
|
|
if (objType == "furniture") return TRUE;
|
|
if (objType == "scripted" && size < 4.0) return TRUE; // Small scripted objects
|
|
|
|
return FALSE;
|
|
}
|
|
|
|
// CORRECTED: Smarter obstacle size estimation
|
|
float estimateObstacleSize(string name, string desc, string objType) {
|
|
string searchText = llToLower(name + " " + desc);
|
|
|
|
// Walls get LARGE sizes (based on keyword priority)
|
|
if (llSubStringIndex(searchText, "wall") >= 0) return 6.0;
|
|
if (llSubStringIndex(searchText, "building") >= 0) return 10.0;
|
|
if (llSubStringIndex(searchText, "fence") >= 0) return 4.0;
|
|
if (llSubStringIndex(searchText, "barrier") >= 0) return 3.0;
|
|
if (llSubStringIndex(searchText, "structure") >= 0) return 5.0;
|
|
|
|
// Furniture gets smaller, navigable sizes
|
|
if (llSubStringIndex(searchText, "table") >= 0) return 1.8;
|
|
if (llSubStringIndex(searchText, "chair") >= 0) return 1.2;
|
|
if (llSubStringIndex(searchText, "lamp") >= 0) return 0.8;
|
|
if (llSubStringIndex(searchText, "pillow") >= 0) return 1.0;
|
|
if (llSubStringIndex(searchText, "ottoman") >= 0) return 1.5;
|
|
if (llSubStringIndex(searchText, "couch") >= 0) return 2.0;
|
|
if (llSubStringIndex(searchText, "bed") >= 0) return 2.5;
|
|
|
|
// Default based on object type
|
|
if (objType == "furniture") return 1.5;
|
|
if (objType == "passive") return 2.5;
|
|
if (objType == "scripted") return 1.8;
|
|
|
|
return 1.8; // Smaller default
|
|
}
|
|
|
|
// Enhanced position blocking check
|
|
integer isPositionBlocked(vector pos) {
|
|
integer count = llGetListLength(obstacle_positions);
|
|
if (count == 0) return FALSE;
|
|
|
|
integer i;
|
|
for (i = 0; i < count; i++) {
|
|
vector obstaclePos = llList2Vector(obstacle_positions, i);
|
|
float obstacleSize = 1.5; // Default size
|
|
if (i < llGetListLength(obstacle_sizes)) {
|
|
obstacleSize = llList2Float(obstacle_sizes, i);
|
|
}
|
|
|
|
// Ensure obstacle size is reasonable
|
|
if (obstacleSize < 0.5) obstacleSize = 1.5;
|
|
|
|
float distance = llVecDist(pos, obstaclePos);
|
|
if (distance < (obstacleSize + OBSTACLE_AVOIDANCE_MARGIN)) {
|
|
return TRUE;
|
|
}
|
|
}
|
|
return FALSE;
|
|
}
|
|
|
|
// Find portals with enhanced logic
|
|
list findPortalsNear(vector position, float searchRadius) {
|
|
list portals = [];
|
|
|
|
if (!ENABLE_PORTAL_DETECTION) return portals;
|
|
if (searchRadius <= 0.0) return portals;
|
|
|
|
// Enhanced search directions
|
|
list directions = [
|
|
<1, 0, 0>, <-1, 0, 0>, // E, W
|
|
<0, 1, 0>, <0, -1, 0>, // N, S
|
|
<0.707, 0.707, 0>, <-0.707, 0.707, 0>, // NE, NW
|
|
<0.707, -0.707, 0>, <-0.707, -0.707, 0>, // SE, SW
|
|
<0.5, 0.866, 0>, <-0.5, 0.866, 0>, // More angles
|
|
<0.866, 0.5, 0>, <-0.866, 0.5, 0>,
|
|
<0.866, -0.5, 0>, <-0.866, -0.5, 0>,
|
|
<0.5, -0.866, 0>, <-0.5, -0.866, 0>
|
|
];
|
|
|
|
integer i;
|
|
for (i = 0; i < llGetListLength(directions); i++) {
|
|
vector dir = llList2Vector(directions, i);
|
|
|
|
// Try multiple distances
|
|
list distances = [searchRadius * 0.3, searchRadius * 0.6, searchRadius];
|
|
integer j;
|
|
for (j = 0; j < llGetListLength(distances); j++) {
|
|
float dist = llList2Float(distances, j);
|
|
vector searchPos = position + (dir * dist);
|
|
|
|
// Check if this direction leads to an opening
|
|
if (!isPositionBlocked(searchPos)) {
|
|
// Verify the opening is wide enough
|
|
vector perpendicular = <-dir.y, dir.x, 0>; // 90-degree rotation
|
|
float halfWidth = PORTAL_MIN_WIDTH * 0.5;
|
|
if (halfWidth > 0.0) {
|
|
vector leftCheck = searchPos + (perpendicular * halfWidth);
|
|
vector rightCheck = searchPos - (perpendicular * halfWidth);
|
|
|
|
if (!isPositionBlocked(leftCheck) && !isPositionBlocked(rightCheck)) {
|
|
// Check if this portal is already in our list
|
|
integer k;
|
|
integer found = FALSE;
|
|
for (k = 0; k < llGetListLength(portals); k++) {
|
|
vector existingPortal = llList2Vector(portals, k);
|
|
if (llVecDist(existingPortal, searchPos) < 2.0) {
|
|
found = TRUE;
|
|
jump next_portal;
|
|
}
|
|
}
|
|
|
|
if (!found) {
|
|
portals += [searchPos];
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Portal found at: " + (string)searchPos);
|
|
}
|
|
}
|
|
|
|
@next_portal;
|
|
}
|
|
}
|
|
}
|
|
}
|
|
}
|
|
|
|
return portals;
|
|
}
|
|
|
|
// Check if we're stuck against walls
|
|
integer isStuckAgainstWall(vector currentPos, vector targetPos) {
|
|
vector dirToTarget = targetPos - currentPos;
|
|
if (llVecMag(dirToTarget) < 0.001) return FALSE;
|
|
|
|
dirToTarget = safeVecNorm(dirToTarget);
|
|
|
|
// Check if direct path is blocked
|
|
vector testPos = currentPos + (dirToTarget * WALL_DETECTION_THRESHOLD);
|
|
if (!isPositionBlocked(testPos)) return FALSE;
|
|
|
|
// Check surrounding directions
|
|
list testDirections = [
|
|
<1, 0, 0>, <-1, 0, 0>, <0, 1, 0>, <0, -1, 0>,
|
|
<0.707, 0.707, 0>, <-0.707, 0.707, 0>,
|
|
<0.707, -0.707, 0>, <-0.707, -0.707, 0>
|
|
];
|
|
|
|
integer blockedCount = 0;
|
|
integer i;
|
|
for (i = 0; i < llGetListLength(testDirections); i++) {
|
|
vector dir = llList2Vector(testDirections, i);
|
|
vector checkPos = currentPos + (dir * WALL_DETECTION_THRESHOLD);
|
|
if (isPositionBlocked(checkPos)) {
|
|
blockedCount++;
|
|
}
|
|
}
|
|
|
|
return (blockedCount >= 5); // More lenient threshold
|
|
}
|
|
|
|
// ---- A* Pathfinding Implementation (FIXED)
|
|
list open_set = [];
|
|
list closed_set = [];
|
|
list all_nodes = [];
|
|
|
|
float calculateHeuristic(vector a, vector b) {
|
|
vector diff = b - a;
|
|
float basicDistance = llFabs(diff.x) + llFabs(diff.y) + (llFabs(diff.z) * 0.3);
|
|
|
|
if (basicDistance < 0.001) basicDistance = 0.001;
|
|
|
|
// Reduced penalty to encourage exploration
|
|
if (isStuckAgainstWall(a, b)) {
|
|
basicDistance *= 1.2;
|
|
}
|
|
|
|
return basicDistance;
|
|
}
|
|
|
|
// Enhanced neighbor generation
|
|
list getNeighbors(vector pos) {
|
|
list neighbors = [];
|
|
|
|
if (GRID_SIZE < 0.5) GRID_SIZE = 2.0;
|
|
|
|
// More movement options
|
|
list offsets = [
|
|
<GRID_SIZE, 0, 0>, <-GRID_SIZE, 0, 0>, // E, W
|
|
<0, GRID_SIZE, 0>, <0, -GRID_SIZE, 0>, // N, S
|
|
<GRID_SIZE, GRID_SIZE, 0>, <-GRID_SIZE, GRID_SIZE, 0>, // NE, NW
|
|
<GRID_SIZE, -GRID_SIZE, 0>, <-GRID_SIZE, -GRID_SIZE, 0>, // SE, SW
|
|
|
|
// Additional diagonal movements for better navigation
|
|
<GRID_SIZE * 0.5, GRID_SIZE * 0.5, 0>,
|
|
<-GRID_SIZE * 0.5, GRID_SIZE * 0.5, 0>,
|
|
<GRID_SIZE * 0.5, -GRID_SIZE * 0.5, 0>,
|
|
<-GRID_SIZE * 0.5, -GRID_SIZE * 0.5, 0>
|
|
];
|
|
|
|
// Add portal-directed movement
|
|
list nearbyPortals = findPortalsNear(pos, PORTAL_SEARCH_RADIUS * 0.5);
|
|
integer j;
|
|
for (j = 0; j < llGetListLength(nearbyPortals); j++) {
|
|
vector portalPos = llList2Vector(nearbyPortals, j);
|
|
vector dirToPortal = portalPos - pos;
|
|
if (llVecMag(dirToPortal) > 0.001) {
|
|
dirToPortal = safeVecNorm(dirToPortal);
|
|
vector portalOffset = dirToPortal * GRID_SIZE;
|
|
offsets += [portalOffset];
|
|
}
|
|
}
|
|
|
|
integer i;
|
|
for (i = 0; i < llGetListLength(offsets); i++) {
|
|
vector offset = llList2Vector(offsets, i);
|
|
vector newPos = pos + offset;
|
|
|
|
// More lenient height restrictions
|
|
if (newPos.z > pos.z + 4.0 || newPos.z < pos.z - 3.0) jump next_neighbor;
|
|
|
|
// Check if position is blocked
|
|
if (!isPositionBlocked(newPos)) {
|
|
neighbors += [newPos];
|
|
}
|
|
|
|
@next_neighbor;
|
|
}
|
|
|
|
return neighbors;
|
|
}
|
|
|
|
// Node finding functions (keep existing optimized versions)
|
|
integer findNodeInOpenSet(vector pos) {
|
|
integer count = llGetListLength(open_set);
|
|
if (count == 0) return -1;
|
|
|
|
integer node_count = (integer)safeDivide((float)count, 5.0);
|
|
integer i;
|
|
|
|
for (i = 0; i < node_count; i++) {
|
|
integer idx = i * 5;
|
|
if (idx < count) {
|
|
vector nodePos = llList2Vector(open_set, idx);
|
|
if (llVecDist(nodePos, pos) < GRID_SIZE * 0.3) {
|
|
return idx;
|
|
}
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
integer findNodeInClosedSet(vector pos) {
|
|
integer count = llGetListLength(closed_set);
|
|
if (count == 0) return -1;
|
|
|
|
integer node_count = (integer)safeDivide((float)count, 5.0);
|
|
|
|
integer i;
|
|
|
|
for (i = 0; i < node_count; i++) {
|
|
integer idx = i * 5;
|
|
if (idx < count) {
|
|
vector nodePos = llList2Vector(closed_set, idx);
|
|
if (llVecDist(nodePos, pos) < GRID_SIZE * 0.3) {
|
|
return idx;
|
|
}
|
|
}
|
|
}
|
|
return -1;
|
|
}
|
|
|
|
integer getLowestFCostNode() {
|
|
integer count = llGetListLength(open_set);
|
|
if (count == 0) return -1;
|
|
|
|
integer bestIndex = 0;
|
|
float bestFCost = llList2Float(open_set, 3);
|
|
|
|
integer node_count = (integer)safeDivide((float)count, 5.0);
|
|
integer i;
|
|
|
|
for (i = 1; i < node_count; i++) {
|
|
integer idx = i * 5;
|
|
if (idx + 3 < count) {
|
|
float fCost = llList2Float(open_set, idx + 3);
|
|
if (fCost < bestFCost) {
|
|
bestFCost = fCost;
|
|
bestIndex = idx;
|
|
}
|
|
}
|
|
}
|
|
|
|
return bestIndex;
|
|
}
|
|
|
|
list reconstructPath(integer goalNodeIndex) {
|
|
list path = [];
|
|
integer currentIndex = goalNodeIndex;
|
|
integer maxIterations = MAX_PATH_WAYPOINTS;
|
|
|
|
// CYCLE DETECTION: Track visited indices to prevent infinite loops
|
|
// If a parent index points back to a node we've already visited,
|
|
// we have a cycle in the parent chain. This should never happen
|
|
// in correct A* operation, but guards against memory corruption
|
|
// or edge case bugs during pathfinding.
|
|
list visited_indices = [];
|
|
|
|
while (currentIndex >= 0 && maxIterations > 0) {
|
|
// CYCLE CHECK: Have we visited this index before?
|
|
if (llListFindList(visited_indices, [currentIndex]) >= 0) {
|
|
// Cycle detected - same node appears twice in parent chain
|
|
llOwnerSay("Pathfinding ERROR: Cycle detected in path reconstruction at index " +
|
|
(string)currentIndex + ". Aborting to prevent infinite loop.");
|
|
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay(" Visited indices: " + llList2CSV(visited_indices));
|
|
llOwnerSay(" Current path length: " + (string)llGetListLength(path));
|
|
}
|
|
|
|
// Return partial path constructed so far
|
|
jump path_done;
|
|
}
|
|
|
|
// Mark this index as visited
|
|
visited_indices += [currentIndex];
|
|
|
|
// Validate index is within bounds
|
|
if (currentIndex < llGetListLength(all_nodes)) {
|
|
vector pos = llList2Vector(all_nodes, currentIndex);
|
|
path = [pos] + path;
|
|
|
|
// Get parent index (stride 5, so parent is at currentIndex + 4)
|
|
if (currentIndex + 4 < llGetListLength(all_nodes)) {
|
|
integer parentIndex = llList2Integer(all_nodes, currentIndex + 4);
|
|
|
|
// Additional validation: parent index should be valid or -1
|
|
if (parentIndex < -1 || parentIndex >= llGetListLength(all_nodes)) {
|
|
llOwnerSay("Pathfinding ERROR: Invalid parent index " + (string)parentIndex +
|
|
" at node index " + (string)currentIndex);
|
|
jump path_done;
|
|
}
|
|
|
|
currentIndex = parentIndex;
|
|
} else {
|
|
// End of list reached
|
|
currentIndex = -1;
|
|
}
|
|
} else {
|
|
// Index out of bounds
|
|
llOwnerSay("Pathfinding ERROR: Node index " + (string)currentIndex +
|
|
" out of bounds (list length: " + (string)llGetListLength(all_nodes) + ")");
|
|
jump path_done;
|
|
}
|
|
|
|
maxIterations--;
|
|
}
|
|
|
|
// Check if we exited due to maxIterations exhaustion (possible infinite loop)
|
|
if (maxIterations == 0 && currentIndex >= 0) {
|
|
llOwnerSay("Pathfinding WARNING: Path reconstruction exceeded " +
|
|
(string)MAX_PATH_WAYPOINTS + " iterations. Path may be incomplete.");
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay(" Final currentIndex: " + (string)currentIndex);
|
|
llOwnerSay(" Path waypoints: " + (string)llGetListLength(path));
|
|
}
|
|
}
|
|
|
|
@path_done;
|
|
|
|
// Sanity check: path length should never exceed MAX_PATH_WAYPOINTS
|
|
if (llGetListLength(path) > MAX_PATH_WAYPOINTS) {
|
|
llOwnerSay("Pathfinding ERROR: Path length (" + (string)llGetListLength(path) +
|
|
") exceeds maximum (" + (string)MAX_PATH_WAYPOINTS + "). Data corruption suspected.");
|
|
// Truncate to safe length
|
|
path = llList2List(path, 0, MAX_PATH_WAYPOINTS - 1);
|
|
}
|
|
|
|
return path;
|
|
}
|
|
|
|
// Main pathfinding algorithm (keep enhanced version but with corrected classification)
|
|
list findPath(vector start, vector goal) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("CORRECTED Pathfinding: Finding path from " + (string)start + " to " + (string)goal);
|
|
}
|
|
|
|
// Safety checks
|
|
if (GRID_SIZE < 0.5) GRID_SIZE = 2.0;
|
|
if (MAX_SEARCH_NODES < 10) MAX_SEARCH_NODES = 500;
|
|
if (PATHFINDING_TIMEOUT < 1.0) PATHFINDING_TIMEOUT = 10.0;
|
|
|
|
// Clear previous search data
|
|
open_set = [];
|
|
closed_set = [];
|
|
all_nodes = [];
|
|
pathfinding_start_time = llGetTime();
|
|
|
|
// Grid snapping with offset to avoid walls
|
|
vector gridStart = <llRound(safeDivide(start.x, GRID_SIZE)) * GRID_SIZE,
|
|
llRound(safeDivide(start.y, GRID_SIZE)) * GRID_SIZE,
|
|
start.z>;
|
|
vector gridGoal = <llRound(safeDivide(goal.x, GRID_SIZE)) * GRID_SIZE,
|
|
llRound(safeDivide(goal.y, GRID_SIZE)) * GRID_SIZE,
|
|
goal.z>;
|
|
|
|
// If start position is blocked, try to find a nearby free position
|
|
if (isPositionBlocked(gridStart)) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Start position blocked, searching for free start position...");
|
|
}
|
|
|
|
list startOffsets = [
|
|
<GRID_SIZE, 0, 0>, <-GRID_SIZE, 0, 0>,
|
|
<0, GRID_SIZE, 0>, <0, -GRID_SIZE, 0>,
|
|
<GRID_SIZE, GRID_SIZE, 0>, <-GRID_SIZE, -GRID_SIZE, 0>
|
|
];
|
|
|
|
integer found = FALSE;
|
|
integer k;
|
|
for (k = 0; k < llGetListLength(startOffsets); k++) {
|
|
vector testStart = gridStart + llList2Vector(startOffsets, k);
|
|
if (!isPositionBlocked(testStart)) {
|
|
gridStart = testStart;
|
|
found = TRUE;
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Found free start position: " + (string)gridStart);
|
|
}
|
|
jump start_found;
|
|
}
|
|
}
|
|
|
|
@start_found;
|
|
if (!found) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Cannot find free start position, using original");
|
|
}
|
|
}
|
|
}
|
|
|
|
// Check if start and goal are too close
|
|
if (llVecDist(gridStart, gridGoal) < GRID_SIZE * 0.5) {
|
|
return [gridStart, gridGoal];
|
|
}
|
|
|
|
// Enhanced goal handling
|
|
if (isPositionBlocked(gridGoal)) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Goal blocked, searching for alternatives...");
|
|
}
|
|
|
|
// Try nearby positions first
|
|
list goalOffsets = [
|
|
<GRID_SIZE, 0, 0>, <-GRID_SIZE, 0, 0>,
|
|
<0, GRID_SIZE, 0>, <0, -GRID_SIZE, 0>,
|
|
<GRID_SIZE, GRID_SIZE, 0>, <-GRID_SIZE, -GRID_SIZE, 0>,
|
|
<GRID_SIZE, -GRID_SIZE, 0>, <-GRID_SIZE, GRID_SIZE, 0>
|
|
];
|
|
|
|
integer found = FALSE;
|
|
integer k;
|
|
for (k = 0; k < llGetListLength(goalOffsets); k++) {
|
|
vector testGoal = gridGoal + llList2Vector(goalOffsets, k);
|
|
if (!isPositionBlocked(testGoal)) {
|
|
gridGoal = testGoal;
|
|
found = TRUE;
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Using nearby goal position: " + (string)gridGoal);
|
|
}
|
|
jump goal_found;
|
|
}
|
|
}
|
|
|
|
@goal_found;
|
|
|
|
// If nearby positions don't work, try portals
|
|
if (!found) {
|
|
list nearbyPortals = findPortalsNear(gridGoal, PORTAL_SEARCH_RADIUS);
|
|
if (llGetListLength(nearbyPortals) > 0) {
|
|
vector closestPortal = llList2Vector(nearbyPortals, 0);
|
|
float closestDist = llVecDist(gridGoal, closestPortal);
|
|
|
|
integer i;
|
|
for (i = 1; i < llGetListLength(nearbyPortals); i++) {
|
|
vector portal = llList2Vector(nearbyPortals, i);
|
|
float dist = llVecDist(gridGoal, portal);
|
|
if (dist < closestDist) {
|
|
closestDist = dist;
|
|
closestPortal = portal;
|
|
}
|
|
}
|
|
|
|
gridGoal = closestPortal;
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Using portal at: " + (string)gridGoal);
|
|
}
|
|
} else {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("No free goal positions or portals found");
|
|
}
|
|
return [];
|
|
}
|
|
}
|
|
}
|
|
|
|
// Initialize start node
|
|
float h_start = calculateHeuristic(gridStart, gridGoal);
|
|
open_set = [gridStart, 0.0, h_start, h_start, -1];
|
|
all_nodes = [gridStart, 0.0, h_start, h_start, -1];
|
|
|
|
integer iterations = 0;
|
|
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Starting A* search from " + (string)gridStart + " to " + (string)gridGoal);
|
|
}
|
|
|
|
while (llGetListLength(open_set) > 0 && iterations < MAX_SEARCH_NODES) {
|
|
// Check timeout
|
|
if (llGetTime() - pathfinding_start_time > PATHFINDING_TIMEOUT) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Pathfinding: Timeout reached after " + (string)iterations + " iterations");
|
|
}
|
|
return [];
|
|
}
|
|
|
|
// Get node with lowest f_cost
|
|
integer currentNodeIndex = getLowestFCostNode();
|
|
if (currentNodeIndex < 0) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("No nodes in open set at iteration " + (string)iterations);
|
|
}
|
|
return [];
|
|
}
|
|
|
|
// Safety check
|
|
if (currentNodeIndex + 4 >= llGetListLength(open_set)) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Index bounds error prevented at iteration " + (string)iterations);
|
|
}
|
|
return [];
|
|
}
|
|
|
|
// Extract current node
|
|
vector currentPos = llList2Vector(open_set, currentNodeIndex);
|
|
float currentGCost = llList2Float(open_set, currentNodeIndex + 1);
|
|
|
|
// Move to closed set
|
|
list currentNode = llList2List(open_set, currentNodeIndex, currentNodeIndex + 4);
|
|
closed_set += currentNode;
|
|
open_set = llDeleteSubList(open_set, currentNodeIndex, currentNodeIndex + 4);
|
|
|
|
// Check if goal reached
|
|
if (llVecDist(currentPos, gridGoal) < GRID_SIZE * 0.8) {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("CORRECTED Pathfinding: Goal reached in " + (string)iterations + " iterations");
|
|
}
|
|
integer allNodesIndex = llListFindList(all_nodes, [currentPos]);
|
|
if (allNodesIndex >= 0) {
|
|
return reconstructPath(allNodesIndex);
|
|
}
|
|
return [gridStart, gridGoal];
|
|
}
|
|
|
|
// Process neighbors
|
|
list neighbors = getNeighbors(currentPos);
|
|
if (DEBUG_PATHFINDING && iterations < 3) {
|
|
llOwnerSay("Iteration " + (string)iterations + ": " + (string)llGetListLength(neighbors) + " neighbors from " + (string)currentPos);
|
|
}
|
|
|
|
integer i;
|
|
for (i = 0; i < llGetListLength(neighbors); i++) {
|
|
vector neighborPos = llList2Vector(neighbors, i);
|
|
|
|
if (findNodeInClosedSet(neighborPos) >= 0) jump next_neighbor;
|
|
|
|
float distance = llVecDist(currentPos, neighborPos);
|
|
if (distance < 0.001) distance = 0.001;
|
|
|
|
float tentativeGCost = currentGCost + distance;
|
|
float hCost = calculateHeuristic(neighborPos, gridGoal);
|
|
float fCost = tentativeGCost + hCost;
|
|
|
|
integer openIndex = findNodeInOpenSet(neighborPos);
|
|
if (openIndex >= 0 && openIndex + 4 < llGetListLength(open_set)) {
|
|
float existingGCost = llList2Float(open_set, openIndex + 1);
|
|
if (tentativeGCost < existingGCost) {
|
|
integer parentIndex = llGetListLength(all_nodes);
|
|
open_set = llListReplaceList(open_set, [neighborPos, tentativeGCost, hCost, fCost, parentIndex],
|
|
openIndex, openIndex + 4);
|
|
}
|
|
} else {
|
|
integer parentIndex = llGetListLength(all_nodes);
|
|
open_set += [neighborPos, tentativeGCost, hCost, fCost, parentIndex];
|
|
}
|
|
|
|
integer parentNodeIndex = llListFindList(all_nodes, [currentPos]);
|
|
all_nodes += [neighborPos, tentativeGCost, hCost, fCost, parentNodeIndex];
|
|
|
|
@next_neighbor;
|
|
}
|
|
|
|
iterations++;
|
|
}
|
|
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("CORRECTED Pathfinding: Failed to find path after " + (string)iterations + " iterations");
|
|
llOwnerSay("Open set size: " + (string)llGetListLength(open_set) + ", Max nodes: " + (string)MAX_SEARCH_NODES);
|
|
}
|
|
return [];
|
|
}
|
|
|
|
// ---- CORRECTED: Process obstacle data with fixed wall classification
|
|
updateObstacleData(string sensesData) {
|
|
obstacle_positions = [];
|
|
obstacle_sizes = [];
|
|
obstacle_details = [];
|
|
wall_segments = [];
|
|
furniture_positions = [];
|
|
|
|
if (sensesData == "") return;
|
|
|
|
list objectStrings = llParseString2List(sensesData, ["|"], []);
|
|
integer i;
|
|
integer wallCount = 0;
|
|
integer furnitureCount = 0;
|
|
|
|
for (i = 0; i < llGetListLength(objectStrings); i++) {
|
|
string objStr = llList2String(objectStrings, i);
|
|
if (objStr == "") jump next_object;
|
|
|
|
list parts = llParseString2List(objStr, ["~"], []);
|
|
|
|
if (llGetListLength(parts) >= 6) {
|
|
string name = llList2String(parts, 0);
|
|
string desc = llList2String(parts, 1);
|
|
string posStr = llList2String(parts, 2);
|
|
float dist = llList2Float(parts, 3);
|
|
key objKey = (key)llList2String(parts, 4);
|
|
string objType = llList2String(parts, 5);
|
|
|
|
list posParts = llParseString2List(posStr, [":"], []);
|
|
if (llGetListLength(posParts) >= 3) {
|
|
float x = llList2Float(posParts, 0);
|
|
float y = llList2Float(posParts, 1);
|
|
float z = llList2Float(posParts, 2);
|
|
|
|
vector pos = <x, y, z>;
|
|
|
|
// Get obstacle size FIRST
|
|
float objSize = estimateObstacleSize(name, desc, objType);
|
|
|
|
// Check for WALL first (priority classification)
|
|
integer isWall = isObstacleWall(pos, objSize, name, desc, objType);
|
|
|
|
// Only check for furniture if NOT already identified as wall
|
|
integer isFurn = FALSE;
|
|
if (!isWall && ENABLE_FURNITURE_DETECTION) {
|
|
isFurn = isFurniture(name, desc, objType, objSize);
|
|
}
|
|
|
|
if (isWall) {
|
|
wallCount++;
|
|
wall_segments += [pos];
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Classified as WALL: " + name + " (size: " + (string)objSize + ")");
|
|
}
|
|
} else if (isFurn) {
|
|
furnitureCount++;
|
|
furniture_positions += [pos];
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Classified as FURNITURE: " + name + " (size: " + (string)objSize + ")");
|
|
}
|
|
} else {
|
|
if (DEBUG_PATHFINDING) {
|
|
llOwnerSay("Classified as OBSTACLE: " + name + " (size: " + (string)objSize + ")");
|
|
}
|
|
}
|
|
|
|
// Store all obstacle data
|
|
obstacle_positions += [pos];
|
|
obstacle_sizes += [objSize];
|
|
obstacle_details += [(string)isWall];
|
|
}
|
|
}
|
|
|
|
@next_object;
|
|
}
|
|
|
|
if (DEBUG_PATHFINDING && llGetListLength(obstacle_positions) > 0) {
|
|
llOwnerSay("CORRECTED Pathfinding: Updated " + (string)llGetListLength(obstacle_positions) +
|
|
" obstacles, " + (string)wallCount + " walls, " + (string)furnitureCount + " furniture");
|
|
}
|
|
}
|
|
|
|
// ---- Load configuration (unchanged)
|
|
integer loadPathfindingConfig() {
|
|
string txt = get_card(CONFIG_CARD);
|
|
if (txt == "") return FALSE;
|
|
|
|
list lines = llParseString2List(txt, ["\n"], []);
|
|
integer i;
|
|
for (i = 0; i < llGetListLength(lines); i++) {
|
|
string line = llStringTrim(llList2String(lines, i), STRING_TRIM);
|
|
if (line == "") jump cont;
|
|
if (llGetSubString(line,0,0) == "#") jump cont;
|
|
if (llGetSubString(line,0,1) == "//") jump cont;
|
|
|
|
integer eq = llSubStringIndex(line, "=");
|
|
if (eq <= 0) jump cont;
|
|
|
|
string k = llStringTrim(llGetSubString(line, 0, eq-1), STRING_TRIM);
|
|
string v = llStringTrim(llGetSubString(line, eq+1, -1), STRING_TRIM);
|
|
|
|
if (k == "PATHFINDING_ENABLED") PATHFINDING_ENABLED = (integer)v;
|
|
else if (k == "GRID_SIZE") {
|
|
float val = (float)v;
|
|
if (val > 0.5) GRID_SIZE = val;
|
|
}
|
|
else if (k == "MAX_SEARCH_NODES") {
|
|
integer val = (integer)v;
|
|
if (val > 10) MAX_SEARCH_NODES = val;
|
|
}
|
|
else if (k == "OBSTACLE_AVOIDANCE_MARGIN") {
|
|
float val = (float)v;
|
|
if (val >= 0.0) OBSTACLE_AVOIDANCE_MARGIN = val;
|
|
}
|
|
else if (k == "MAX_PATH_WAYPOINTS") {
|
|
integer val = (integer)v;
|
|
if (val > 0) MAX_PATH_WAYPOINTS = val;
|
|
}
|
|
else if (k == "PATHFINDING_TIMEOUT") {
|
|
float val = (float)v;
|
|
if (val > 0.0) PATHFINDING_TIMEOUT = val;
|
|
}
|
|
else if (k == "DEBUG_PATHFINDING") DEBUG_PATHFINDING = (integer)v;
|
|
else if (k == "WALL_SIZE_THRESHOLD") {
|
|
float val = (float)v;
|
|
if (val > 0.0) WALL_SIZE_THRESHOLD = val;
|
|
}
|
|
else if (k == "ENABLE_SIZE_BASED_WALLS") ENABLE_SIZE_BASED_WALLS = (integer)v;
|
|
else if (k == "ENABLE_FURNITURE_DETECTION") ENABLE_FURNITURE_DETECTION = (integer)v;
|
|
else if (k == "WALL_DETECTION_THRESHOLD") {
|
|
float val = (float)v;
|
|
if (val > 0.0) WALL_DETECTION_THRESHOLD = val;
|
|
}
|
|
else if (k == "PORTAL_SEARCH_RADIUS") {
|
|
float val = (float)v;
|
|
if (val > 0.0) PORTAL_SEARCH_RADIUS = val;
|
|
}
|
|
else if (k == "PORTAL_MIN_WIDTH") {
|
|
float val = (float)v;
|
|
if (val > 0.0) PORTAL_MIN_WIDTH = val;
|
|
}
|
|
else if (k == "ENABLE_WALL_FOLLOWING") ENABLE_WALL_FOLLOWING = (integer)v;
|
|
else if (k == "ENABLE_PORTAL_DETECTION") ENABLE_PORTAL_DETECTION = (integer)v;
|
|
else if (k == "CORNER_ESCAPE_DISTANCE") {
|
|
float val = (float)v;
|
|
if (val > 0.0) CORNER_ESCAPE_DISTANCE = val;
|
|
}
|
|
else if (k == "MAX_WALL_FOLLOW_ATTEMPTS") {
|
|
integer val = (integer)v;
|
|
if (val > 0) MAX_WALL_FOLLOW_ATTEMPTS = val;
|
|
}
|
|
|
|
@cont;
|
|
}
|
|
return TRUE;
|
|
}
|
|
|
|
// ---- DEFAULT
|
|
default {
|
|
state_entry() {
|
|
// Initialize with safe defaults
|
|
if (GRID_SIZE < 0.5) GRID_SIZE = 2.0;
|
|
if (MAX_SEARCH_NODES < 10) MAX_SEARCH_NODES = 500;
|
|
if (PATHFINDING_TIMEOUT < 1.0) PATHFINDING_TIMEOUT = 10.0;
|
|
if (WALL_SIZE_THRESHOLD < 2.0) WALL_SIZE_THRESHOLD = 4.0;
|
|
|
|
loadPathfindingConfig();
|
|
llOwnerSay("Pathfinding: CORRECTED Wall Classification System ready");
|
|
}
|
|
|
|
link_message(integer sender, integer num, string str, key id) {
|
|
if (num == LM_CONFIG) {
|
|
NPC = id;
|
|
}
|
|
else if (num == LM_TIMER_CONFIG) {
|
|
// Pathfinding will be triggered by Actions.lsl, no timer changes needed
|
|
llOwnerSay("Pathfinding: Master timer coordination configured");
|
|
}
|
|
else if (num == LM_SENSES_UPDATE) {
|
|
updateObstacleData(str);
|
|
}
|
|
else if (num == LM_PATH_REQUEST) {
|
|
if (!PATHFINDING_ENABLED || pathfinding_busy) {
|
|
llMessageLinked(LINK_SET, LM_PATH_RESULT, "FAILED", id);
|
|
return;
|
|
}
|
|
|
|
pathfinding_busy = TRUE;
|
|
|
|
list parts = llParseString2List(str, ["|"], []);
|
|
if (llGetListLength(parts) >= 2) {
|
|
vector start = (vector)llList2String(parts, 0);
|
|
vector goal = (vector)llList2String(parts, 1);
|
|
|
|
if (start == ZERO_VECTOR || goal == ZERO_VECTOR) {
|
|
llMessageLinked(LINK_SET, LM_PATH_RESULT, "FAILED", id);
|
|
pathfinding_busy = FALSE;
|
|
return;
|
|
}
|
|
|
|
list path = findPath(start, goal);
|
|
|
|
if (llGetListLength(path) > 0) {
|
|
string pathStr = "";
|
|
integer i;
|
|
for (i = 0; i < llGetListLength(path); i++) {
|
|
if (i > 0) pathStr += "|";
|
|
pathStr += (string)llList2Vector(path, i);
|
|
}
|
|
llMessageLinked(LINK_SET, LM_PATH_RESULT, pathStr, id);
|
|
} else {
|
|
llMessageLinked(LINK_SET, LM_PATH_RESULT, "FAILED", id);
|
|
}
|
|
} else {
|
|
llMessageLinked(LINK_SET, LM_PATH_RESULT, "FAILED", id);
|
|
}
|
|
|
|
pathfinding_busy = FALSE;
|
|
}
|
|
}
|
|
} |