[Quy Anh] «Elliot» Nguyen
98 lines
3.6 KiB
C++
98 lines
3.6 KiB
C++
#pragma once
|
|
#include "vector2d_float.hpp"
|
|
|
|
struct Obstacle {
|
|
Vector2d start; // First endpoint of the obstacle segment.
|
|
Vector2d end; // Second endpoint of the obstacle segment.
|
|
Vector2d obstacle_v; // Direction vector of the segment: end - start.
|
|
float length_sq_inv; // Inverse squared length of the segment, used for projection.
|
|
|
|
// Default constructor: creates a degenerate obstacle at the origin.
|
|
// This is mainly used for array allocation before assigning real obstacles.
|
|
Obstacle() : start(0, 0), end(0, 0), obstacle_v(0, 0), length_sq_inv(0.0) {}
|
|
|
|
// Constructs an obstacle as a finite line segment from s to e.
|
|
//
|
|
// The segment direction and inverse squared length are precomputed and cached because
|
|
// they are reused when projecting pedestrian positions onto the obstacle.
|
|
//
|
|
// @param s The first endpoint of the obstacle segment.
|
|
// @param e The second endpoint of the obstacle segment.
|
|
//
|
|
Obstacle(Vector2d s, Vector2d e) : start(s), end(e) {
|
|
obstacle_v = end - start;
|
|
float length_sq = obstacle_v.dot(obstacle_v);
|
|
length_sq_inv = length_sq != 0.0 ? 1.0 / length_sq : 0.0;
|
|
}
|
|
|
|
// Flop count: 5 ADDs, 5 MULTs
|
|
//
|
|
// Finds the point on this finite obstacle segment that is closest to p.
|
|
//
|
|
// The method projects the vector (p - start) onto the obstacle direction
|
|
// vector, then clamps the projection factor to [0, 1] so that the returned
|
|
// point lies on the finite segment.
|
|
//
|
|
// Note : If the obstacle is a single point, meaning start == end, length_sq_inv is zero
|
|
// and the method returns start.
|
|
//
|
|
// @param p The point for which the closest obstacle point is computed.
|
|
// @return The closest point on the obstacle segment to p.
|
|
//
|
|
Vector2d find_nearest_obstacle_point(const Vector2d &p) const {
|
|
|
|
// # 3 ADDs, 3 MULTs
|
|
float projection = (p - start).dot(obstacle_v) * length_sq_inv;
|
|
|
|
if (projection < 0.0)
|
|
projection = 0.0;
|
|
else if (projection > 1.0)
|
|
projection = 1.0;
|
|
|
|
// # 2 ADDs, 2 MULTs
|
|
return start + obstacle_v * projection;
|
|
}
|
|
};
|
|
|
|
struct Board {
|
|
float length; // Board length.
|
|
float width; // Board width.
|
|
Obstacle *obstacles; // Array of obstacles contained in the board.
|
|
int n_obstacles; // Total number of obstacles
|
|
|
|
// Constructs a rectangular board of size length x width.
|
|
//
|
|
// The input array o contains n user-defined obstacles. The constructor copies
|
|
// those obstacles and then appends the two horizontal borders of the board (bottom and top) as additional
|
|
// obstacle segments:
|
|
//
|
|
// bottom: (0, 0) -> (length, 0)
|
|
// top: (0, width) -> (length, width)
|
|
//
|
|
// Note: only the horizontal borders are modeled as obstacles. The vertical
|
|
// borders are left open so pedestrians can enter and leave the walkway without
|
|
// being affected by repulsive forces from those borders. This is consistent
|
|
// with Fig. 2 in the paper, where the horizontal walkway boundaries are
|
|
// emphasized but the vertical ends are not shown as walls.
|
|
//
|
|
// @param l The board length.
|
|
// @param w The board width.
|
|
// @param n The number of user-defined obstacles in o.
|
|
// @param o Array of user-defined obstacles to copy into the board obstacles array.
|
|
//
|
|
Board(float l, float w, int n, Obstacle *o) : length(l), width(w), n_obstacles(n + 2) {
|
|
obstacles = new Obstacle[n + 2];
|
|
|
|
for (int i = 0; i < n; i++) {
|
|
obstacles[i] = o[i];
|
|
}
|
|
|
|
// Board borders: bottom, top.
|
|
obstacles[n] = Obstacle(Vector2d(0, 0), Vector2d(l, 0));
|
|
obstacles[n + 1] = Obstacle(Vector2d(0, w), Vector2d(l, w));
|
|
}
|
|
|
|
// Releases the dynamically allocated obstacle array.
|
|
~Board() { delete[] obstacles; }
|
|
};
|