// origin, direction, cone threshold
void AgentList::objects_in_cone(const Vec3& position, const Vec3& direction, float theta)
{
Vec3 p = translate_position(position);
Vec3 v = vec3_normalize(direction);
unsigned int num = 0;
for (size_t i=0; i<this->max; i++)
{
Agent* a = &this->objects[i];
if (a->id == this->null_id) continue;
Vec3 q = quadrant_translate_position(p, a->get_position());
Vec3 at = vec3_normalize(vec3_sub(q, p));
float ip = vec3_dot(at, v);
float arc = abs(acosf(ip));
if (arc < theta)
filtered_objects[num++] = a;
}
this->n_filtered = num;
}
