void Ship::setApproach(sf::Vector2f pos) { stop(); approaching = true; target_angle = getAngleBetween(getPosition(), pos); target = pos; }
pair<Vec3, Vec3> RotateDesktopGesture::calculateCameraPosition() { // Right Direction Vec3 referenceDirection(-1.0f, 0.0f, 0.0f); Vec3 position, direction; Vec3 startPosition, endPosition; Vec3 startDirection, endDirection; int startWallIndex, endWallIndex; float ratio = 1.0f; Vec3 camDirection(cam->getDir().x, cam->getDir().z, 0.0f); Vec3 camPosition(cam->getEye().x, cam->getEye().z, 0.0f); float angle = getAngleBetween(camDirection, referenceDirection); if (camDirection.y > 0.0f) { // The camera is pointing in the upper quadrants if (angle < 90.0f) { //first quadrant ratio = angle / 90.0f; startWallIndex = 2; endWallIndex = 0; } else if (angle >= 90.0f && angle <= 180.0f) { //second quadrant ratio = (angle - 90.0f) / 90.0f; startWallIndex = 0; endWallIndex = 3; } } else { // The camera is pointing in the lower quadrants if (angle < 90.0f) { //fourth quadrant ratio = angle / 90.0f; startWallIndex = 2; endWallIndex = 1; } else if (angle >= 90.0f && angle <= 180.0f) { //third quadrant ratio = (angle - 90.0f) / 90.0f; startWallIndex = 1; endWallIndex = 3; } } // Find the two points in the world to interpolate between cam->lookAtWall(GLOBAL(Walls)[startWallIndex], startPosition, startDirection); cam->lookAtWall(GLOBAL(Walls)[endWallIndex], endPosition, endDirection); position = lerp(startPosition, endPosition, ratio); direction = lerp(startDirection, endDirection, ratio); return make_pair(position, direction); }
double getLineDistance(const std::vector<double> &p1, const std::vector<double> &p2) { double sqDist = 0; for (unsigned int i = 0; i < p1.size(); i++) { sqDist += pow(getAngleBetween(p1[i], p2[i]), 2); } return sqrt(sqDist); }
int RotateDesktopGesture::getWallCameraIsFacing() { Vec3 camDirection(cam->getDir().x, cam->getDir().z, 0.0f); Vec3 camPosition(cam->getEye().x, cam->getEye().z, 0.0f); Vec3 referenceDirection(-1.0f, 0.0f, 0.0f); float angle = getAngleBetween(camDirection, referenceDirection); Vec3 desktopDims = GetDesktopBox().GetExtents(); if (camDirection.y > 0.0f) { float frontLeftAngle = getAngleBetween(Vec3(desktopDims.x, desktopDims.z, 0.0f) - camPosition, referenceDirection); float frontRightAngle = getAngleBetween(Vec3(-desktopDims.x, desktopDims.z, 0.0f) - camPosition, referenceDirection); if (angle < frontRightAngle) { return 2; } else if (angle >= frontRightAngle && angle < frontLeftAngle) { return 0; } else if (angle >= frontLeftAngle) { return 3; } } else { float backRightAngle = getAngleBetween(Vec3(-desktopDims.x, -desktopDims.z, 0.0f) - camPosition, referenceDirection); float backLeftAngle = getAngleBetween(Vec3(desktopDims.x, -desktopDims.z, 0.0f) - camPosition, referenceDirection); if (angle < backRightAngle) { return 2; } else if (angle >= backRightAngle && angle < backLeftAngle) { return 1; } else if (angle >= backLeftAngle) { return 3; } } return -1; }
std::vector<double> getPointBetween(const std::vector<double> &p1, const std::vector<double> &p2, double dist) { std::vector<double> new_point(p1.size()); double fraction = dist / getLineDistance(p2, p1); for (unsigned int i = 0; i < p1.size(); i++) { new_point[i] = p1[i] + fraction * getDirectionMultiplier(p1[i], p2[i]) * getAngleBetween(p1[i], p2[i]); } return new_point; }
double SSLGamePlanner::orientToOpponentGoal(uint8_t id) { geometry_msgs::Point32 goal_pos; goal_pos.x = -3.025 + 2* 3.025*OUR_FIELD; goal_pos.y = 0; goal_pos.z = 0; geometry_msgs::Point32 ball_pos; ball_pos.x = ball_state.position.x; ball_pos.y = ball_state.position.y; ball_pos.z = ball_state.position.z; return getAngleBetween(ball_pos, goal_pos); }
void Ship::approach() { target_angle = getAngleBetween(getPosition(), target); turnTo(Angle(target_angle)); if (getDistanceBetween(getPosition(), target) < getGlobalBounds().width * 2.0f) { stop(); setRotation(target_angle); } else { correctSpeed(); } }
void Ship::action(Chunk<Entity> * chunk) { for (auto& e : chunk->entities) { if (e->getOwner() != this->getOwner()) { if (shooting) { shoot(getAngleBetween(getPosition(), e->getPosition()), chunk); shooting = true; } break; } } awaiting_action = false; }
bool Frustum::clipLine(const FrustumPlane &p, osg::Vec3 &v1, osg::Vec3 v2) { osg::Vec3 result; { osg::Vec3 vLineDir = v2-v1; vLineDir.normalize(); double dist1 = p.plane.distance(v1); double dist2 = p.plane.distance(v2); if (dist1*dist2 >= 0) return false; // punkte schneiden diese plane nicht double numerator = -dist1; double denominator = p.plane.getNormal() * vLineDir; if (0.0 == denominator) result = v1; else { double dist = numerator/denominator; result = v1 + (vLineDir*dist); } } // so, jetzt gucken, ob das ding im polygon sitzt { osg::Vec3Array* vertices = p.getVertices(); double angle = 0.0; int vertCount = vertices->size(); osg::Vec3 a,b; for(int i = 0; i< vertCount; i++) { a = result - (*vertices)[i]; b = result - (*vertices)[(i+1) % vertCount]; angle += getAngleBetween(a,b); } if (angle >= (2*osg::PI*0.9999)) { v1 = result; return true; } } return false; }
void SolarSystem::setObjectsVelocity() { std::list<std::pair<SolarObject*, SolarObject*> > pairs = getSolarObjectsPairs(); for(std::list<std::pair<SolarObject*, SolarObject*> >::iterator it = pairs.begin(); it != pairs.end(); ++it) { std::pair<SolarObject*, SolarObject*> pair = *it; SolarObject* first = pair.first; SolarObject* second = pair.second; double distance = getDistanceBetween(first, second); double force = GravityC * first->mass * second->mass / distance / distance / 6046; double angle = getAngleBetween(first, second); double dv1 = force / first->mass * timeStep; double dv2 = force / second->mass * timeStep; first->changeVelocity(dv1, angle); second->changeVelocity(dv2, angle + PI); } }
std::vector<Instance*> Layer::getInstancesInCircleSegment(const ModelCoordinate& center, uint16_t radius, int32_t sangle, int32_t eangle) { std::vector<Instance*> instances; ExactModelCoordinate exactCenter(center.x, center.y); std::vector<Instance*> tmpInstances = getInstancesInCircle(center, radius); int32_t s = (sangle + 360) % 360; int32_t e = (eangle + 360) % 360; bool greater = (s > e) ? true : false; for (std::vector<Instance*>::iterator it = tmpInstances.begin(); it != tmpInstances.end(); ++it) { int32_t angle = getAngleBetween(exactCenter, intPt2doublePt((*it)->getLocationRef().getLayerCoordinates())); if (greater) { if (angle >= s || angle <= e) { instances.push_back(*it); } } else { if (angle >= s && angle <= e) { instances.push_back(*it); } } } return instances; }
std::vector<std::vector<double> > interpolate(const std::vector<double> &p1, const std::vector<double> &p2, double step_size) { std::vector<std::vector<double> > interpolation; double dist = getLineDistance(p1, p2); if (dist > step_size) { double fraction = step_size / dist; std::vector<double> diffs(p1.size()); std::vector<double> point(p1.size()); for (unsigned int i = 0; i < p1.size(); i++) { diffs[i] = getAngleBetween(p1[i], p2[i]); } for (unsigned int i = 1; fraction * i < 1.0; i++) { for (unsigned int j = 0; j < point.size(); j++) { point[j] = p1[j] + fraction * i * getDirectionMultiplier(p1[j], p2[j]) * diffs[j]; } interpolation.push_back(point); } } return interpolation; }
std::pair<roadPtr, float> IntersectionGeometry::getBest(float angle, bool searchClockwise) { //Find angle with largest -ve change std::list<float>::iterator angleItr = angles.begin(); std::list<roadPtr>::iterator roadItr = connected.begin(); float best = boost::math::float_constants::two_pi; float bestAngle = 0.0f; roadPtr bestRoad; while (angleItr != angles.end()) { if ((angle == *angleItr)) { angleItr++; roadItr++; continue; //Iterating over ourself! } //bool searchClockwise = false; //bool isAngleAbove = isAbove(angle); //if (scanRight && isAngleAbove) searchClockwise = true; //else if (!scanRight && !isAngleAbove) searchClockwise = true; float dif = getAngleBetween(angle, *angleItr, searchClockwise); //float value = scanLeft ? *angleItr - angle : angle - *angleItr; //float dif = fmodf(value, boost::math::float_constants::two_pi); //Wrap to +ve value if (dif < 0.0f) dif += boost::math::float_constants::two_pi; if (dif < best) { best = dif; bestAngle = *angleItr; bestRoad = *roadItr; } angleItr++; roadItr++; } return std::make_pair(bestRoad, bestAngle); }
geometry_msgs::Vector3 SSLPathPlanner::exePlanForRobot (const int& id) { geometry_msgs::Point curr_point = getCurrentPosition (id); geometry_msgs::Point next_point; next_point.x = next_target_poses[id].x; next_point.y = next_target_poses[id].y; next_point.z = 0.0; geometry_msgs::Point target_point; target_point.x = pose_control.pose[id].pose.x; target_point.y = pose_control.pose[id].pose.y; target_point.z = 0.0; double curr_theta = team_state_[id].pose.theta; // std::cout<<curr_theta<<std::endl; double goal_theta = next_target_poses[id].theta; // std::cout<<goal_theta<<std::endl; double angular_diff = getAngularDifference (curr_theta, goal_theta); // std::cout<<"angular_diff: "<<angular_diff/ssl::math::PI*180.0<<std::endl; double angular_vel; if (fabs (angular_diff) > ssl::math::PI / 2.0) angular_vel = -MAX_ANGULAR_VEL * ssl::math::sign (angular_diff); else angular_vel = -MAX_ANGULAR_VEL * (angular_diff / (ssl::math::PI / 2.0)); // angular_vel = ssl::math::sign(angular_diff)*MAX_ANGULAR_VEL*(1-exp(-(fabs(angular_diff)))); markWaypoint (next_point, 0, id); double dist = sqrt (getSquaredDistance (curr_point, next_point)); double ang = getAngleBetween (curr_point, next_point); // if(dist<VERY_CRITICAL_DIST) // { // geometry_msgs::Vector3 v; // double v_mag = dist/CRITICAL_DISTANCE * CRITICAL_LINEAR_VEL; // v.x = v_mag * cos(goal_theta); // v.y = v_mag * sin(goal_theta); // return v; // } double vel_x = 0; double vel_y = 0; bool transit_pass = true; if (next_point.x == target_point.x && next_point.y == target_point.y && next_point.z == target_point.z) { transit_pass = false; } if (dist > CRITICAL_DISTANCE) { vel_x = MAX_LINEAR_VEL * cos (ang); vel_y = MAX_LINEAR_VEL * sin (ang); //now consider obstacles around } else { if (!transit_pass) { if (dist < VERY_CRITICAL_DIST) { vel_x = CRITICAL_LINEAR_VEL * (dist / VERY_CRITICAL_DIST) * cos (ang); vel_y = CRITICAL_LINEAR_VEL * (dist / VERY_CRITICAL_DIST) * sin (ang); } else { vel_x = MAX_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * cos (ang); vel_y = MAX_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * sin (ang); } } else { //TODO optimize this vel_x = INT_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * cos (ang); vel_y = INT_LINEAR_VEL * (1 - exp (-dist * EXPONENT)) * sin (ang); } } geometry_msgs::Vector3 v = getCurrentVelocity (id); // double curr_vel = getVelMagnitude(v); // double curr_vel_ang= getVelAngle(v); double des_acc_x = (vel_x - v.x) / ssl::config::TIME_STEP_SEC; double des_acc_y = (vel_y - v.y) / ssl::config::TIME_STEP_SEC; double des_acc = sqrt (des_acc_x * des_acc_x + des_acc_y * des_acc_y); double des_acc_ang = atan2 (des_acc_y, des_acc_x); if (des_acc > MAX_LINEAR_ACC) des_acc = MAX_LINEAR_ACC; des_acc_x = MAX_LINEAR_ACC * cos (des_acc_ang); des_acc_y = MAX_LINEAR_ACC * sin (des_acc_ang); v.x += des_acc_x * ssl::config::TIME_STEP_SEC * ACC_TUNING; v.y += des_acc_y * ssl::config::TIME_STEP_SEC * ACC_TUNING; double des_vel_ang = getVelAngle (v); double des_vel_mag = getVelMagnitude (v); if (des_vel_mag > MAX_LINEAR_VEL) des_vel_mag = MAX_LINEAR_VEL; v.x = des_vel_mag * cos (des_vel_ang); v.y = des_vel_mag * sin (des_vel_ang); tf::Vector3 tf_v; tf::vector3MsgToTF (v, tf_v); tf_v = tf_v.rotate (tf::Vector3 (0, 0, 1), -curr_theta); tf::vector3TFToMsg (tf_v, v); v.z = angular_vel; return v; }
void LayerCache::updateEntry(LayerCache::Entry& item) { if(item.instance_index == -1) { return; } RenderItem& render_item = m_instances[item.instance_index]; Instance* instance = render_item.instance; ExactModelCoordinate map_coords = instance->getLocationRef().getMapCoordinates(); DoublePoint3D screen_position = m_camera->toVirtualScreenCoordinates(map_coords); render_item.facing_angle = getAngleBetween(instance->getLocationRef(), instance->getFacingLocation()); int32_t angle = static_cast<int32_t>(m_camera->getRotation()) + render_item.facing_angle + instance->getRotation(); ImagePtr image; Action* action = instance->getCurrentAction(); int32_t w = 0; int32_t h = 0; if(!action) { // Try static images then default action. int32_t image_id = render_item.getStaticImageIndexByAngle(angle, instance); if(image_id == -1) { if (!instance->getObject()->isStatic()) { action = instance->getObject()->getDefaultAction(); } } else { image = ImageManager::instance()->get(image_id); } } item.force_update = (action != 0); if(action) { AnimationPtr animation = action->getVisual<ActionVisual>()->getAnimationByAngle( render_item.facing_angle + static_cast<int32_t>(m_camera->getRotation())); unsigned animation_time = instance->getActionRuntime() % animation->getDuration(); image = animation->getFrameByTimestamp(animation_time); int32_t action_frame = animation->getActionFrame(); if (action_frame != -1) { if (render_item.image != image) { if (action_frame == animation->getFrameIndex(animation_time)) { instance->callOnActionFrame(action, action_frame); } } } int32_t facing_angle = render_item.facing_angle; if (facing_angle < 0) { facing_angle += 360; } instance->setRotation(facing_angle); m_needupdate = true; } if (image) { w = image->getWidth(); h = image->getHeight(); screen_position.x -= w / 2; screen_position.x += image->getXShift(); screen_position.y -= h / 2; screen_position.y += image->getYShift(); } render_item.image = image; if (render_item.screenpoint == screen_position) { return; } render_item.screenpoint = screen_position; render_item.bbox.x = static_cast<int32_t>(screen_position.x); render_item.bbox.y = static_cast<int32_t>(screen_position.y); render_item.bbox.w = w; render_item.bbox.h = h; render_item.dimensions = render_item.bbox; CacheTree::Node* node = m_tree->find_container(render_item.bbox); if (node) { if(item.node) { item.node->data().erase(item.entry_index); } item.node = node; node->data().insert(item.entry_index); } }
// Returns true if the two vectors are parallel and point in the same direction, // given a certain error threshold in degrees. bool Gesture::isSameDirection(Vec3 &firstVector, Vec3 &secondVector, float threshold) { float angle = getAngleBetween(firstVector, secondVector); return (angle < threshold); }