static void draw_circle(running_machine *machine, bitmap_t* bitmap)
{
starshp1_state *state = machine->driver_data<starshp1_state>();
int cx = get_circle_hpos(state);
int cy = get_circle_vpos(state);
int x = 0;
int y = get_radius(state);
/* Bresenham's circle algorithm */
int d = 3 - 2 * get_radius(state);
while (x <= y)
{
draw_circle_line(machine, bitmap, cx, cy - x, y);
draw_circle_line(machine, bitmap, cx, cy + x, y);
draw_circle_line(machine, bitmap, cx, cy - y, x);
draw_circle_line(machine, bitmap, cx, cy + y, x);
x++;
if (d < 0)
d += 4 * x + 6;
else
d += 4 * (x - y--) + 10;
}
}
Rect2 CapsuleShape2D::get_rect() const {
Vector2 he = Point2(get_radius(), get_radius() + get_height() * 0.5);
Rect2 rect;
rect.position = -he;
rect.size = he * 2.0;
return rect;
}
inline bool sphere_aabb_intersection( const Sphere& s, const AABB& aabb )
{
using point_t = typename geometric_traits<Sphere>::point_type;
using length_t = typename geometric_traits<point_t>::arithmetic_type;
using vector_t = vector<length_t, dimension_of<point_t>::value>;
auto d2 = point_aabb_distance_sqrd(get_center(s), aabb);
return d2 <= get_radius(s) * get_radius(s);
}
inline bool sphere_aabb_intersection( const Sphere& s, const AABB& aabb, Point& p )
{
using point_t = typename geometric_traits<Sphere>::point_type;
using length_t = typename geometric_traits<point_t>::arithmetic_type;
using vector_t = vector<length_t, dimension_of<point_t>::value>;
assign(p, point_aabb_closest_point(get_center(s), aabb));
//! Sphere and AABB intersect if the distance from sphere center to point p is less than the sphere radius.
auto v = vector_t{ p - get_center(s) };
return magnitude_sqrd(v) <= get_radius(s) * get_radius(s);
}
bool Light::intersect(const BoundingBox &box) const
{
glm::vec3 tmp;
float total_distance;
tmp = glm::abs(get_position() - box.get_center());
tmp -= box.get_half_size();
tmp = glm::max(tmp, 0.0f);
total_distance = glm::length2(tmp);
return total_distance <= (get_radius() * get_radius());
}
/** Returns a point inside the cylinder,
specified by its location relative to the cylinder axis,
its relative radius and its rotation angle about the axis
@param relative_height a number in the range [0..1] that specifies
the point location relative to the cylinder axis
such that 0 specifies the cylinder bottom and
1 specifies its top
@param relative_radius a number in the range [0..1] that specifies
the distance of the point from the cylinder axis
relative to the cylinder radius, 0 being on the
axis itself, and 1 being on the cylinder surface
@param angle angle in radians about the cylinder axis, with 0 set to
an arbitrary but consistent direction
*/
const Vector3D Cylinder3D::get_inner_point_at(double relative_height,
double relative_radius,
double angle) const {
// compute the point relative to cylinder of equivalent dimensions,
// but whose main axis segment is z-aligned and lying at the origin
Vector3D scaling(get_radius() * relative_radius,
get_radius() * relative_radius, get_segment().get_length());
Vector3D pt(scaling[0] * sin(angle), scaling[1] * cos(angle),
scaling[2] * relative_height);
// transform the cylinder axis from the origin to the correct location
Rotation3D rot = get_rotation_taking_first_to_second(
Vector3D(0, 0, 1),
get_segment().get_point(1) - get_segment().get_point(0));
Transformation3D tr(rot, get_segment().get_point(0));
return tr.get_transformed(pt);
}
void CpuPointSet::purturb_points (size_t group_size, float purturb_factor)
{
ASSERT_DIVIDES(group_size, size);
const size_t num_probes = size / group_size;
const size_t block_size = dim * group_size;
float sigma = purturb_factor * get_radius() / sqrt(dim);
LOG("purturbing point groups of size " << group_size << " by " << sigma);
Vector<int8_t> noise(block_size);
generate_noise(noise, sigma);
int8_t * restrict dx = noise;
for (size_t i_probe = 0; i_probe < num_probes; ++i_probe) {
uint8_t * restrict x = m_points + block_size * i_probe;
for (size_t i = 0; i < block_size; ++i) {
x[i] = bound_to(0, 255, int(x[i]) + int(dx[i]));
}
}
}
/**
* @brief 内包する Droplet の相対位置を計算する
*/
void
Splitter::calc_relative_positions ()
{
std::size_t n_droplets = m_segm_list.size();
double darg = 2.0 * M_PI / n_droplets; // それぞれの Droplet を配置するときの角度
// 初期の法線ベクトルを決める
Glib::Rand& rand = Droplet::get_random();
Vector<double> norm(rand.get_double_range(-1.0, 1.0),
rand.get_double_range(-1.0, 1.0));
norm.set_unit();
// 各 Droplet の相対座標を求める
std::list<Segment>::iterator it;
for( it = m_segm_list.begin(); it != m_segm_list.end(); ++it )
{
// 相対位置座標を決める
it->rel_pos = (get_radius() - it->droplet->get_radius()) * norm;
// 速度は外向きランダム
it->droplet->set_random_velocity(norm);
norm.set_rotated(darg); // norm を darg だけ回転させる
}
}
bool circle::contains(const shape &point) {
const auto d = pow((point.get_x() - get_x()), 2.0) + pow((point.get_y() - get_y()), 2.0);
// TODO: cache rsquared
//return d <= this.rSquared;
return d <= pow( /*TODO FIX THIS BY TAKING A CIRCLE AS PARAM */ (get_radius()*2 + get_radiussize()*2), 2.0);
}
bool bounding_sphere::contains(const point & point) const
{
xvector dist(origin - point);
float squared = math::point(XMVector3Dot(dist, dist))[axis::x];
return squared < std::pow(get_radius(), 2);
}
Light::Light(const LightData &light_data): m_light_data(light_data),
m_inv_sqr_radius(0.0f)
{
set_inv_sqr_radius(get_radius());
update_bounding_box();
update_rgb9_e5_color();
}
bool bounding_sphere::intersects(const sphere & sphere) const
{
xvector dist(origin - sphere.origin);
float squared = math::point(XMVector3Dot(dist, dist))[axis::x];
float radius_sum = get_radius() + sphere.radius;
return squared <= std::pow(radius_sum, 2);
}
double searchY(double X) {
double lo = 0, hi = MAX_RANGE;
int i;
for(i = 0; i < MAX_ITER; i++) {
double a = (2 * lo + hi) / 3;
double b = (lo + 2 * hi) / 3;
double ra = get_radius(X, a);
double rb = get_radius(X, b);
if(ra < rb) hi = b;
else lo = a;
}
return get_radius(X, lo);
}
virtual void updateBounds(double origin_x, double origin_y, double origin_z, double* min_x, double* min_y, double* max_x, double* max_y){
boost::recursive_mutex::scoped_lock lock(lock_);
std::string global_frame = layered_costmap_->getGlobalFrameID();
transformed_people_.clear();
for(unsigned int i=0; i<people_list_.people.size(); i++){
people_msgs::Person& person = people_list_.people[i];
people_msgs::Person tpt;
geometry_msgs::PointStamped pt, opt;
try{
pt.point.x = person.position.x;
pt.point.y = person.position.y;
pt.point.z = person.position.z;
pt.header.frame_id = people_list_.header.frame_id;
tf_.transformPoint(global_frame, pt, opt);
tpt.position.x = opt.point.x;
tpt.position.y = opt.point.y;
tpt.position.z = opt.point.z;
pt.point.x += person.velocity.x;
pt.point.y += person.velocity.y;
pt.point.z += person.velocity.z;
tf_.transformPoint(global_frame, pt, opt);
tpt.velocity.x = tpt.position.x - opt.point.x;
tpt.velocity.y = tpt.position.y - opt.point.y;
tpt.velocity.z = tpt.position.z - opt.point.z;
transformed_people_.push_back(tpt);
double mag = sqrt(pow(tpt.velocity.x,2) + pow(person.velocity.y, 2));
double factor = 1.0 + mag * factor_;
double point = get_radius(cutoff_, amplitude_, covar_ * factor );
*min_x = std::min(*min_x, tpt.position.x - point);
*min_y = std::min(*min_y, tpt.position.y - point);
*max_x = std::max(*max_x, tpt.position.x + point);
*max_y = std::max(*max_y, tpt.position.y + point);
}
catch(tf::LookupException& ex) {
ROS_ERROR("No Transform available Error: %s\n", ex.what());
continue;
}
catch(tf::ConnectivityException& ex) {
ROS_ERROR("Connectivity Error: %s\n", ex.what());
continue;
}
catch(tf::ExtrapolationException& ex) {
ROS_ERROR("Extrapolation Error: %s\n", ex.what());
continue;
}
}
}
void Light::update_bounding_box(const float scale)
{
BoundingBox bounding_box;
bounding_box.set_center(get_position());
bounding_box.set_half_size(glm::vec3(get_radius()));
bounding_box.scale(scale);
set_bounding_box(bounding_box);
}
static int circle_collision(starshp1_state *state, const rectangle *rect)
{
int center_x = get_circle_hpos(state);
int center_y = get_circle_vpos(state);
int r = get_radius(state);
return point_in_circle(rect->min_x, rect->min_y, center_x, center_y, r) ||
point_in_circle(rect->min_x, rect->max_y, center_x, center_y, r) ||
point_in_circle(rect->max_x, rect->min_y, center_x, center_y, r) ||
point_in_circle(rect->max_x, rect->max_y, center_x, center_y, r);
}
void CpuPointSet::quantize (
const Point & probe,
Vector<float> & likes) const
{
ASSERT_SIZE(probe, dim);
ASSERT_SIZE(likes, size);
measure(probe);
Vector<float> & sd = m_temp_squared_distances;
Cpu::quantize_one(get_radius(), sd, likes);
}
void CircleShape2D::draw(const RID& p_to_rid,const Color& p_color) {
Vector<Vector2> points;
for(int i=0;i<24;i++) {
points.push_back(Vector2(Math::cos(i*Math_PI*2/24.0),Math::sin(i*Math_PI*2/24.0))*get_radius());
}
Vector<Color> col;
col.push_back(p_color);
VisualServer::get_singleton()->canvas_item_add_polygon(p_to_rid,points,col);
}
bool bounding_sphere::intersects(const frustum & frustum) const
{
auto & planes = frustum.get_planes();
for (const plane & plane : planes)
{
const point & dot = XMPlaneDotCoord(plane, origin);
if (dot[axis::x] <= -get_radius())
return false;
}
return true;
}
void CpuPointSet::quantize_batch (
const Point & probes,
Vector<float> & likes) const
{
ASSERT_DIVIDES(dim, probes.size);
const size_t num_probes = probes.size / dim;
ASSERT_SIZE(likes, size * num_probes);
ASSERT_LE(num_probes, max_batch_size);
Vector<float> sd(size * num_probes, m_temp_squared_distances_batch);
Cpu::measure_batch(probes, m_points, sd, num_probes);
Cpu::quantize_batch(get_radius(), sd, likes, num_probes);
}
bool circle::intersects(const rectangle &range) {
const auto xDist = abs(range.get_x() - get_x());
const auto yDist = abs(range.get_y() - get_y());
// radius of the circle
const auto r = get_radius() + get_radiussize();
const auto w = range.get_width();
const auto h = range.get_height();
const auto edges = pow((xDist - w), 2) + pow((yDist - h), 2);
// no intersection
if (xDist > (r + w) || yDist > (r + h))
return false;
// intersection within the circle
if (xDist <= w || yDist <= h)
return true;
// intersection on the edge of the circle
//return edges <= this.rSquared;
return edges <= pow(get_radius() + get_radiussize(), 2); // ^- TODO: cache rSquared
}
bool bounding_sphere::intersects(const ray & ray) const
{
point m = ray.get_origin() - origin;
point c = point(XMVector3Dot(m, m)) - std::pow(get_radius(), 2);
if (c <= 0.f)
return true;
point b = XMVector3Dot(m, ray.get_direction());
if (b > 0.f)
return false;
float disc = std::pow(b[axis::x], 2) - c[axis::x];
return disc >= 0.f;
}
void geometry_coordinate()
{
typedef psyq::geometry::ball<template_coordinate> ball_type;
auto const local_ball(
ball_type::make(template_coordinate::make_filled(2), 10));
auto const local_ball_aabb(
psyq::geometry::make_aabb(local_ball));
auto local_point(
ball_type::point::make(template_coordinate::make_filled(2)));
local_point = ball_type::point::make(template_coordinate::make_filled(3));
typedef typename psyq::geometry::line_segment<template_coordinate>
line_segment_type;
auto const local_line(
line_segment_type::make(
local_ball.center_,
template_coordinate::make(local_ball.get_radius(), -4, 3)));
auto const local_line_aabb(psyq::geometry::make_aabb(local_line));
typedef psyq::geometry::ray<template_coordinate> ray_type;
ray_type const local_ray(local_line);
auto const local_ray_aabb(
psyq::geometry::make_aabb(local_ray));
typedef psyq::geometry::box<template_coordinate> box_type;
auto const local_box(
box_type::make_cuboid(
local_line.origin_.get_position(),
local_line.direction_.get_unit(),
60 * 3.1415926535f / 180,
template_coordinate::make_filled(1)));
auto const local_box_aabb(
psyq::geometry::make_aabb(local_box));
typedef psyq::geometry::barycentric_triangle<template_coordinate> triangle_type;
auto const local_triangle(
triangle_type::make(
template_coordinate::make(0, 0, 0),
template_coordinate::make(1, 0, 0),
template_coordinate::make(0, 1, 0)));
}
void CpuPointSet::accum_stats (const Point & probes, size_t num_probes)
{
ASSERT_SIZE(probes, dim * num_probes);
ASSERT_LE(num_probes, max_batch_size);
Vector<float> sd(size * num_probes, m_temp_squared_distances_batch);
Vector<float> likes(size * num_probes, m_temp_likes_batch);
Cpu::measure_batch(probes, m_points, sd, num_probes);
m_quantize_stats_accum += Cpu::quantize_batch(
get_radius(),
sd,
likes,
num_probes);
m_construct_stats_accum += construct_deriv(num_probes);
m_count_stats_accum += num_probes;
accum_prior(num_probes);
}
Vector<Vector3> SphereShape::_gen_debug_mesh_lines() {
float r = get_radius();
Vector<Vector3> points;
for (int i = 0; i <= 360; i++) {
float ra = Math::deg2rad((float)i);
float rb = Math::deg2rad((float)i + 1);
Point2 a = Vector2(Math::sin(ra), Math::cos(ra)) * r;
Point2 b = Vector2(Math::sin(rb), Math::cos(rb)) * r;
points.push_back(Vector3(a.x, 0, a.y));
points.push_back(Vector3(b.x, 0, b.y));
points.push_back(Vector3(0, a.x, a.y));
points.push_back(Vector3(0, b.x, b.y));
points.push_back(Vector3(a.x, a.y, 0));
points.push_back(Vector3(b.x, b.y, 0));
}
return points;
}
bool bounding_sphere::intersects(const ray & ray, intersection_data & out) const
{
point m = ray.get_origin() - origin;
point b = point(XMVector3Dot(m, ray.get_direction()));
point c = point(XMVector3Dot(m, m)) - std::pow(get_radius(), 2);
if (c > 0.f && b > 0.f)
return false;
float disc = std::pow(b[axis::x], 2) - c[axis::x];
if (disc < 0.f)
return false;
out.distance = -b[axis::x] - std::sqrt(disc);
if (out.distance < 0.f)
out.distance = 0.f;
out.coordinates = ray.get_origin() + point(out.distance) * float3(ray.get_direction());
return true;
}
sphere bounding_sphere::get_sphere() const
{
return sphere(origin, get_radius());
}
Rect2 CircleShape2D::get_rect() const {
Rect2 rect;
rect.position = -Point2(get_radius(), get_radius());
rect.size = Point2(get_radius(), get_radius()) * 2.0;
return rect;
}
Declaration::TurnPoint::TurnPoint(const OrderedTaskPoint &tp)
:waypoint(tp.GetWaypoint()),
shape(get_shape(tp)), radius(get_radius(tp))
{
}
bool bounding_sphere::intersects(const box & box) const
{
return box.squared_distance_to(origin) <= std::pow(get_radius(), 2);
}
