double Triangle::area() const {
Point p1 = *points.get(0), p2 = *points.get(1), p3 = *points.get(2);
double a = point_dist(p1, p2), b = point_dist(p2, p3),
c = point_dist(p3, p1);
double s = 0.5 * (a + b + c);
return sqrt(s * (s - a) * (s - b) * (s - c));
}
point* map_find_closest(point *p, int item)
{
if (items[item] == NULL)
return NULL;
// find the closest bucket
point *closest_bucket = &items[item]->p;
float closest_bucket_dist = point_dist(p, closest_bucket);
point_node *pn = items[item]->next;
while (pn) {
float dist = point_dist(p, &pn->p);
if (dist < closest_bucket_dist) {
closest_bucket_dist = dist;
closest_bucket = &pn->p;
}
pn = pn->next;
}
return closest_bucket;
}
building* building_find_closest(point *p, int model)
{
building *closest = NULL;
float closest_dist = INT_MAX;
for (building *b = buildings; b != NULL; b = b->next)
{
if (b->model->model != model)
continue;
float dist = point_dist(&b->p, p);
if (dist < closest_dist) {
closest = b;
closest_dist = dist;
}
}
return closest;
}
/*
* update the actor's location according to:
* -moving towards their leader
* -staying a minimum distance from other actors
*/
void actor_update(Actor *a, Actor *others, int nothers){
double genX, genY, genZ;
double dist, closest[2];
double distToOthers, alpha, distToBoss;
double leaderSpeed = vector_length(&(a->boss->velocity));
int i, verbose = 0;
if(verbose) printf("updating actor\n");
Actor closestActors[2];
Vector toBoss, awayFromOthers, heading;
Point otherAvgLoc;
genX = a->dispersion - ((double)rand() / RAND_MAX) * 2.0 * a->dispersion;
genY = a->dispersion - ((double)rand() / RAND_MAX) * 2.0 * a->dispersion;
genZ = a->dispersion - ((double)rand() / RAND_MAX) * 2.0 * a->dispersion;
// find closest two actors
closest[0] = closest[1] = DBL_MAX;
for(i=0; i<nothers; i++){
dist = point_dist(&(others[i].location), &(a->location));
if(others[i].id != a->id && (dist < closest[0])) {
closest[0] = dist;
closestActors[0] = others[i];
}
}
for(i=0; i<nothers; i++){
dist = point_dist(&(others[i].location), &(a->location));
if(others[i].id != a->id && (dist < closest[1]) && (dist != closest[0])) {
closest[1] = dist;
closestActors[1] = others[i];
}
}
// handle case where no other actors
if(closest[0] == DBL_MAX)
vector_set(&heading,a->boss->location.val[0] - a->location.val[0],
a->boss->location.val[1] - a->location.val[1],
a->boss->location.val[2] - a->location.val[2]);
else {
// handle case where only one other actor
if(closest[1] == DBL_MAX)
vector_set(&awayFromOthers, a->location.val[0] - closestActors[0].location.val[0],
a->location.val[1] - closestActors[0].location.val[1],
a->location.val[2] - closestActors[0].location.val[2]);
// calculate vector away from average position of others
else{
point_avg(&otherAvgLoc, &(closestActors[0].location), &(closestActors[1].location));
vector_set(&awayFromOthers, a->location.val[0] - otherAvgLoc.val[0],
a->location.val[1] - otherAvgLoc.val[1],
a->location.val[2] - otherAvgLoc.val[2] );
}
distToOthers = vector_length(&awayFromOthers);
vector_normalize(&awayFromOthers);
// calculate vector to leader
vector_set(&toBoss, a->boss->location.val[0] - a->location.val[0],
a->boss->location.val[1] - a->location.val[1],
a->boss->location.val[2] - a->location.val[2]);
distToBoss = vector_length(&toBoss);
vector_normalize(&toBoss);
// andrew suggests vector blending with toBoss and awayFromOthers
alpha = 0.0; // just going toward boss
if(distToOthers < a->minDist || distToBoss <= a->minDist){
alpha = 1.0; // just going away from others
} else if(distToOthers <= a->thresholdDist){
alpha = (a->thresholdDist-distToOthers) /
(a->thresholdDist-a->minDist);
}
vector_set(&heading,(1-alpha) * toBoss.val[0] + alpha * awayFromOthers.val[0],
(1-alpha) * toBoss.val[1] + alpha * awayFromOthers.val[1],
(1-alpha) * toBoss.val[2] + alpha * awayFromOthers.val[2] );
}
vector_normalize(&heading);
// update location according to new heading with jitter and speed
a->location.val[0] += (heading.val[0]) * leaderSpeed + genX;
a->location.val[1] += (heading.val[1]) * leaderSpeed + genY;
a->location.val[2] += (heading.val[2]) * leaderSpeed + genZ;
}
void
NavGraph::edge_add_split_intersection(const NavGraphEdge &edge)
{
std::list<std::pair<cart_coord_2d_t, NavGraphEdge>> intersections;
const NavGraphNode &n1 = node(edge.from());
const NavGraphNode &n2 = node(edge.to());
try {
for (const NavGraphEdge &e : edges_) {
cart_coord_2d_t ip;
if (e.intersection(n1.x(), n1.y(), n2.x(), n2.y(), ip)) {
// we need to split the edge at the given intersection point,
// and the new line segments as well
intersections.push_back(std::make_pair(ip, e));
}
}
std::list<std::list<std::pair<cart_coord_2d_t, NavGraphEdge> >::iterator> deletions;
for (auto i1 = intersections.begin(); i1 != intersections.end(); ++i1) {
const std::pair<cart_coord_2d_t, NavGraphEdge> &p1 = *i1;
const cart_coord_2d_t &c1 = p1.first;
const NavGraphEdge &e1 = p1.second;
const NavGraphNode &n1_from = node(e1.from());
const NavGraphNode &n1_to = node(e1.to());
for (auto i2 = std::next(i1); i2 != intersections.end(); ++i2) {
const std::pair<cart_coord_2d_t, NavGraphEdge> &p2 = *i2;
const cart_coord_2d_t &c2 = p2.first;
const NavGraphEdge &e2 = p2.second;
if (points_different(c1.x, c1.y, c2.x, c2.y)) continue;
float d = 1.;
if (e1.from() == e2.from() || e1.from() == e2.to()) {
d = point_dist(n1_from.x(), n1_from.y(), c1.x, c1.y);
} else if (e1.to() == e2.to() || e1.to() == e2.from()) {
d = point_dist(n1_to.x(), n1_to.y(), c1.x, c1.y);
}
if (d < 1e-4) {
// the intersection point is the same as a common end
// point of the two edges, only keep it once
deletions.push_back(i1);
break;
}
}
}
for (auto d = deletions.rbegin(); d != deletions.rend(); ++d) {
intersections.erase(*d);
}
if (intersections.empty()) {
NavGraphEdge e(edge);
e.set_property("created-for", edge.from() + "--" + edge.to());
add_edge(e, EDGE_FORCE);
} else {
Eigen::Vector2f e_origin(n1.x(), n1.y());
Eigen::Vector2f e_target(n2.x(), n2.y());
Eigen::Vector2f e_dir = (e_target - e_origin).normalized();
intersections.sort([&e_origin, &e_dir](const std::pair<cart_coord_2d_t, NavGraphEdge> &p1,
const std::pair<cart_coord_2d_t, NavGraphEdge> &p2)
{
const Eigen::Vector2f p1p(p1.first.x, p1.first.y);
const Eigen::Vector2f p2p(p2.first.x, p2.first.y);
const float k1 = e_dir.dot(p1p - e_origin);
const float k2 = e_dir.dot(p2p - e_origin);
return k1 < k2;
});
std::string en_from = edge.from();
cart_coord_2d_t ec_from(n1.x(), n1.y());
std::string prev_to;
for (const auto &i : intersections) {
const cart_coord_2d_t &c = i.first;
const NavGraphEdge &e = i.second;
// add intersection point (if necessary)
NavGraphNode ip = closest_node(c.x, c.y);
if (! ip || points_different(c.x, c.y, ip.x(), ip.y())) {
ip = NavGraphNode(gen_unique_name(), c.x, c.y);
add_node(ip);
}
// if neither edge end node is the intersection point, split the edge
if (ip.name() != e.from() && ip.name() != e.to()) {
NavGraphEdge e1(e.from(), ip.name(), e.is_directed());
NavGraphEdge e2(ip.name(), e.to(), e.is_directed());
remove_edge(e);
e1.set_properties(e.properties());
e2.set_properties(e.properties());
add_edge(e1, EDGE_FORCE, /* allow existing */ true);
add_edge(e2, EDGE_FORCE, /* allow existing */ true);
// this is a special case: we might intersect an edge
// which has the same end node and thus the new edge
// from the intersection node to the end node would
// be added twice
prev_to = e.to();
}
// add segment edge
if (en_from != ip.name() && prev_to != ip.name()) {
NavGraphEdge e3(en_from, ip.name(), edge.is_directed());
e3.set_property("created-for", en_from + "--" + ip.name());
add_edge(e3, EDGE_FORCE, /* allow existing */ true);
}
en_from = ip.name();
ec_from = c;
}
if (en_from != edge.to()) {
NavGraphEdge e3(en_from, edge.to(), edge.is_directed());
e3.set_property("created-for", en_from + "--" + edge.to());
add_edge(e3, EDGE_FORCE, /* allow existing */ true);
}
}
} catch (Exception &ex) {
throw Exception("Failed to add edge %s-%s%s: %s",
edge.from().c_str(), edge.is_directed() ? ">" : "-", edge.to().c_str(),
ex.what_no_backtrace());
}
}
double RANSACLine::distance_from_midpoint(const RANSACPoint &point) {
double mpt_x = (x1 + x2) / 2;
double mpt_y = (y1 + y2) / 2;
return point_dist(mpt_x, mpt_y, point.x, point.y);
}
double RANSACLine::length() { return point_dist(x1, y1, x2, y2); }
double point_dist(Point a, Point b)
{
return point_dist(a - b);
}
double line_length(Line l)
{
return point_dist(l.a - l.b);
}