static void grid_apply_function(grid *grid_ptr, double2 center, double radius,
int (*cb_fun)(grid *, grid_callback_data), void *user_data, int n_excluded,
const int *excluded_indices)
{
/*
* Apply a callback function `cb_fun` to all points with a distance less than `radius` to the point `center`
* except for those that are contained in `excluded_indices`. The information about the search position and the
* current point are passed to the callback function using the `grid_callback_data` struct.
* Returns if all points are processed or the callback function returns a non-zero value.
*/
int i, j, k, l;
double2 top_right_position, bottom_left_position;
int2 top_right_cell, bottom_left_cell;
grid_callback_data callback_data;
top_right_position.x = center.x + radius;
top_right_position.y = center.y + radius;
bottom_left_position.x = center.x - radius;
bottom_left_position.y = center.y - radius;
top_right_cell = grid_get_cell(grid_ptr, &top_right_position);
bottom_left_cell = grid_get_cell(grid_ptr, &bottom_left_position);
callback_data.center_point = center;
callback_data.r = radius;
callback_data.user_data = user_data;
for (i = bottom_left_cell.y; i <= top_right_cell.y; i++)
{
for (j = bottom_left_cell.x; j <= top_right_cell.x; j++)
{
int cell_index = i * grid_ptr->num_cells.x + j;
for (k = grid_ptr->cell_offsets[cell_index]; k < grid_ptr->cell_offsets[cell_index + 1]; k++)
{
for (l = 0; l < n_excluded; l++)
{
if (excluded_indices[l] == k)
{
break;
}
}
if (l < n_excluded)
{
continue;
}
if (distance_sq(center, grid_ptr->points[k]) >= radius * radius)
{
continue;
}
callback_data.current_index = k;
callback_data.current_point = grid_ptr->points[k];
if (cb_fun(grid_ptr, callback_data))
{
return;
}
}
}
}
}
static int grid_cb_nearest_neighbor(grid *grid_ptr, grid_callback_data callback_data)
{
/*
* Callback function to find the index and distance of the nearest neighbor to `center`.
*/
double distance;
double2 *data = (double2 *)callback_data.user_data;
double2 center = callback_data.center_point;
double2 point = callback_data.current_point;
(void)grid_ptr;
distance = distance_sq(center, point);
if (distance > 0 && (distance < data->x || data->x < 0))
{
data->x = distance_sq(center, point);
data->index = callback_data.current_index;
}
return 0;
}
static double2 calculate_ball_center(double2 point1, double2 point2, double r)
{
/*
* Calculate the center of a ball with radius `r` which has `point1` and `point2` on its circumference.
*/
double length;
double distance = distance_sq(point1, point2) / 4;
double h = sqrt(r * r - distance);
double2 m, m_normalized, center;
m.x = (point2.x - point1.x) / 2;
m.y = (point2.y - point1.y) / 2;
length = sqrt(m.x * m.x + m.y * m.y);
m_normalized.x = m.x / length;
m_normalized.y = m.y / length;
center.x = point1.x + m.x + h * m_normalized.y;
center.y = point1.y + m.y - h * m_normalized.x;
return center;
}
/**
* Merge a point cloud in the internal model
*
* @param cloud: point cloud in the custom frame
* @param inter: an internal container of cells
*/
void atlaas::rasterize(const points& cloud, cells_info_t& inter) const {
size_t index;
float z_mean, n_pts, new_z;
// merge point-cloud in internal structure
for (const auto& point : cloud) {
index = meta.index_custom(point[0], point[1]);
if (index >= inter.size() )
continue; // point is outside the map
auto& info = inter[ index ];
new_z = point[2];
n_pts = info[N_POINTS];
if (n_pts < 1) {
info[N_POINTS] = 1;
info[Z_MAX] = new_z;
info[Z_MIN] = new_z;
info[Z_MEAN] = new_z;
info[VARIANCE] = 0;
info[DIST_SQ] = distance_sq(sensor_xy, {{point[0], point[1]}});
} else {
z_mean = info[Z_MEAN];
// increment N_POINTS
info[N_POINTS]++;
// update Z_MAX
if (new_z > info[Z_MAX])
info[Z_MAX] = new_z;
// update Z_MIN
if (new_z < info[Z_MIN])
info[Z_MIN] = new_z;
/* Incremental mean and variance updates (according to Knuth's bible,
Vol. 2, section 4.2.2). The actual variance will later be divided
by the number of samples plus 1. */
info[Z_MEAN] = (z_mean * n_pts + new_z) / info[N_POINTS];
info[VARIANCE] += (new_z - z_mean) * (new_z - info[Z_MEAN]);
}
}
}
Real AABB::distance(const Real3& pos) const
{
return sqrt(distance_sq(pos));
}
void target_closest_enemy::execute_leaf_goal_callback(tree::execution_occurence occ, tree::state_of_traversal& t) const {
if (occ == tree::execution_occurence::LAST)
return;
auto& cosmos = t.step.cosm;
auto subject = t.subject;
auto pos = position(subject);
auto& los = t.step.transient.calculated_line_of_sight.at(subject);
auto& attitude = subject.get<components::attitude>();
entity_id closest_hostile_raw;
float min_distance = std::numeric_limits<float>::max();
for (auto s_raw : los.visible_sentiences) {
auto s = cosmos[s_raw];
auto att = calculate_attitude(s, subject);
if (att == attitude_type::WANTS_TO_KILL || att == attitude_type::WANTS_TO_KNOCK_UNCONSCIOUS) {
auto dist = distance_sq(s, subject);
if (dist < min_distance) {
closest_hostile_raw = s;
min_distance = dist;
}
}
}
auto closest_hostile = cosmos[closest_hostile_raw];
attitude.currently_attacked_visible_entity = closest_hostile;
if (closest_hostile.alive()) {
attitude.is_alert = true;
attitude.last_seen_target_position_inspected = false;
attitude.last_seen_target_position = position(closest_hostile);
attitude.last_seen_target_velocity = velocity(closest_hostile);
}
auto crosshair = subject[sub_entity_name::CHARACTER_CROSSHAIR];
if (crosshair.alive()) {
auto& crosshair_offset = crosshair.get<components::crosshair>().base_offset;
float vel1 = assess_projectile_velocity_of_weapon(subject[slot_function::PRIMARY_HAND].get_item_if_any());
float vel2 = assess_projectile_velocity_of_weapon(subject[slot_function::SECONDARY_HAND].get_item_if_any());
float vel = std::max(vel1, vel2);
if (vel > 1.0 && closest_hostile.alive()) {
vec2 leaded;
if (velocity(closest_hostile).length_sq() > 1)
leaded = position(closest_hostile) + velocity(closest_hostile) * distance(closest_hostile, subject) / vel;// direct_solution(position(closest_hostile), velocity(closest_hostile), vel);
else
leaded = position(closest_hostile);
crosshair_offset = leaded - position(subject);
}
else if (is_entity_physical(subject)) {
crosshair_offset = velocity(subject).length() > 3.0 ? velocity(subject) : vec2(10, 0);
}
else
crosshair_offset = vec2(10, 0);
}
}
inline double distance(const Point& pA, const Point& pB) {
return std::sqrt(distance_sq(pA, pB));
}
inline bool line3<T>::is_between_points(const vec3<T>& point, const vec3<T>& begin, const vec3<T>& stop) const
{
const T f = length_sq(stop - begin);
return distance_sq(point, begin) <= f && distance_sq(point, stop) <= f;
}
inline T line3<T>::get_length_sq() const
{
return distance_sq(start,end);
}
inline float distance(const Point& pA, const Point& pB) {
return std::sqrt(distance_sq(pA, pB));
}
/**
* calculate the distance between positions
* this function is a part of the trait of ParticleSpace.
* @param pos1
* @param pos2
* @return the distance
*/
inline Real distance(const Real3& pos1, const Real3& pos2) const
{
return std::sqrt(distance_sq(pos1, pos2));
}
inline typename detail::element_type_of< T1_ >::type distance(
T1_ const& p1, T2_ const& p2 )
{
return std::sqrt( distance_sq( p1, p2 ) );
}
inline Real distance(const Real3& pos1, const Real3& pos2, const Real3& edge_lengths) const
{
return std::sqrt(distance_sq(pos1, pos2, edge_lengths));
}
