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);
}
