// Assumes 0 <= B < T
LossData WCV_tvar::WCV_interval(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi, double B, double T) const {
double time_in = T;
double time_out = B;
Vect2 so2 = so.vect2();
Vect2 si2 = si.vect2();
Vect2 s2 = so2.Sub(si2);
Vect2 vo2 = vo.vect2();
Vect2 vi2 = vi.vect2();
Vect2 v2 = vo2.Sub(vi2);
double sz = so.z-si.z;
double vz = vo.z-vi.z;
Interval ii = wcv_vertical->vertical_WCV_interval(table.getZTHR(),table.getTCOA(),B,T,sz,vz);
if (ii.low > ii.up) {
return LossData(time_in,time_out);
}
Vect2 step = v2.ScalAdd(ii.low,s2);
if (Util::almost_equals(ii.low,ii.up)) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
if (horizontal_WCV(step,v2)) {
time_in = ii.low;
time_out = ii.up;
}
return LossData(time_in,time_out);
}
LossData ld = horizontal_WCV_interval(ii.up-ii.low,step,v2);
time_in = ld.getTimeIn() + ii.low;
time_out = ld.getTimeOut() + ii.low;
return LossData(time_in,time_out);
}
void SolveAccelerationProblems1()
{
//1. A flower pot falls from a windowsill 25.0 m above the sidewalk.
// How much time does a person below have to move out of the way?
// How fast is the flower pot moving when it strikes the ground?
Meter const potfalls( 25. );
SquareSecond const falloffsquaretime = (potfalls * 2.) / Acceleration3( 9.8 );
Second const rep1 = sqrt( falloffsquaretime );
Velocity const rep1_2 = potfalls / rep1;
Scalar const _rep1 = rep1.GetValue();
Scalar const _rep1_2 = rep1_2.GetValue();
Assert( fequal( _rep1, 2.2587697572631282 ) );
Assert( fequal( _rep1_2, 11.067971810589327 ) );
//2. A plane, starting from rest at one end of a runway, undergoes a
// constant acceleration of 1.6 m/s2 for a distance of 1600 m before
// takeoff. What is its speed upon takeoff? What is the time required
// for takeoff?
Meter const planedistance( 1600. );
SquareSecond const squaretime2 = (planedistance * 2.) / Acceleration3( 1.6 );
Second const rep2 = sqrt( squaretime2 );
Velocity const rep2_2 = planedistance / rep2;
Scalar const _rep2 = rep2.GetValue();
Scalar const _rep2_2 = rep2_2.GetValue();
Assert( fequal( _rep2, 44.721359549995796 ) );
Assert( fequal( _rep2_2, 35.777087639996637 ) );
}
/**********************************************************
* SHIP :: interact()
***********************************************************/
void Ship :: interact(const Interface & pUI, void * asteroids)
{
orientation += pUI.isLeft() * TURN_RADIUS;
orientation -= pUI.isRight() * TURN_RADIUS;
if (pUI.isUp())
{
thrustOn = true;
Velocity thrust;
thrust.setDx(THRUST *std::cos(deg2rad(orientation + 90)));
thrust.setDy(THRUST * std::sin(deg2rad(orientation + 90)));
v += thrust;
}
else
thrustOn = false;
if (pUI.isSpace())
{
Asteroids * pAsteroids = (Asteroids *)asteroids;
pAsteroids->addItem(new Bullet(v, orientation));
}
}
void Receiver1D::removeDirectArrival(const Source& source,
const Velocity& velocity, float t_width)
{
float dt = this->getDt();
int half_len = t_width / dt;
/// get the velocity average in a range
int z_min = std::min(source.getDepth(), this->getDepth());
int z_max = std::max(source.getDepth(), this->getDepth());
int s[] = {z_min, 0};
int e[] = {z_max, velocity.getNx()};
std::vector<int> start(s, s + 2);
std::vector<int> end(e, e + 2);
float vel_avg = velocity.getAverageRange(start, end);
const int nx = this->getGeometryDim()[0];
for (int ix = 0; ix < nx; ix++) {
std::vector<int> currRcvLoc, sourceLoc;
sourceLoc.push_back(source.getDepth());
sourceLoc.push_back(source.getGeometryOrigin()[0]);
currRcvLoc.push_back(getDepth());
currRcvLoc.push_back(getGeometryOrigin()[0] + getGeometryDelta()[0] * ix);
const float dist2 = variance(sourceLoc, currRcvLoc);
int tstart = std::sqrt(dist2 * vel_avg);
int tend = ((tstart + 2 * half_len) > this->getNt()) ? this->getNt() : (tstart + 2 * half_len);
float *p = &mData[ix * getNt()];
std::fill(p + tstart, p + tend, 0);
}
}
/**********************************************************
* MEDIUMROCK constructor
***********************************************************/
MediumRock :: MediumRock(Velocity bigV)
{
setPoint(bigV.getX(), bigV.getY());
v.setDx(random(-MED_SPEED, MED_SPEED));
v.setDy(random(-MED_SPEED, MED_SPEED));
v += bigV;
}
/**********************************************************
* SMALLROCK constructor
***********************************************************/
SmallRock :: SmallRock(Velocity biggerV)
{
setPoint(biggerV.getX(), biggerV.getY());
v.setDx(random(-SM_SPEED, SM_SPEED));
v.setDy(random(-SM_SPEED, SM_SPEED));
v += biggerV;
}
XYZ principia__BubbleVelocityCorrection(Plugin const* const plugin,
int const reference_body_index) {
journal::Method<journal::BubbleVelocityCorrection> m({plugin,
reference_body_index});
CHECK_NOTNULL(plugin);
Velocity<World> const result =
plugin->BubbleVelocityCorrection(reference_body_index);
return m.Return(ToXYZ(result.coordinates() / (Metre / Second)));
}
bool KinematicIntegerBands::conflict(Detection3D* det, const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi,
double B, double T) {
if (Util::almost_equals(B,T)) {
Vect3 sot = vo.ScalAdd(B,so);
Vect3 sit = vi.ScalAdd(B,si);
return det->violation(sot,vo,sit,vi);
}
return det->conflict(so,vo,si,vi,B,T);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::vsAccel(const LatLonAlt& so, const Velocity& vo, double t, double a) {
double dist = vo.gs()*t;
double currentTrk = vo.trk();
LatLonAlt sn = GreatCircle::linear_initial(so, currentTrk, dist);
double nsz = so.alt() + vo.z*t + 0.5*a*t*t;
sn = sn.mkAlt(nsz);
Velocity vn = vo.mkVs(vo.z + a*t);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::gsAccel(const LatLonAlt& so, const Velocity& vo, double t, double a) {
double dist = vo.gs()*t + 0.5*a*t*t;
double currentTrk = vo.trk();
LatLonAlt sn = GreatCircle::linear_initial(so, currentTrk, dist);
sn = sn.mkAlt(so.alt() + vo.z*t);
double vnGs = vo.gs() + a*t;
Velocity vn = vo.mkGs(vnGs);
//fpln(" $$$$$ gsAccel: sn = "+sn+" vn = "+vn);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
bool WCV_tvar::violation(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) const {
Vect2 so2 = so.vect2();
Vect2 si2 = si.vect2();
Vect2 s2 = so2.Sub(si2);
Vect2 vo2 = vo.vect2();
Vect2 vi2 = vi.vect2();
Vect2 v2 = vo2.Sub(vi2);
return horizontal_WCV(s2,v2) &&
wcv_vertical->vertical_WCV(table.getZTHR(),table.getTCOA(),so.z-si.z,vo.z-vi.z);
}
Velocity PlanningProblem::getControl(uint i)
{
Velocity c;
if(i < trajec.length()) {
Station st = trajec.getStation(i);
c = st.getVelocity();
}
else
c.setZero();
return c;
}
void Item::move()
{
Velocity v = this->getVelocity();
Point slope = v.getSlope();
Point location = this->getLocation();
location.addX(slope.getX());
location.addY(slope.getY());
this->setLocation(location);
}
Velocity Solver::getRandomVelocity()
{
Velocity velocity;
for (int i = 0; i < currentProblem->n; ++i) {
double randomVariable = getRandomDoubleValue(0.0, 1.0);
velocity.push_back(randomVariable);
}
return velocity;
}
void Solver::findSolution()
{
for (auto &i : swarm.getParticles()) {
Velocity newVelocity;
Solution newPosition;
/**
* Iterate through all dimensions of a Solution/position
* to calculate the new velocity and update the position.
*
* j = dimension
*/
for (int j = 0; j < currentProblem->n; ++j) {
int currentPositionD = i.getPosition().at(j);
double currentVelocityD = i.getVelocity().at(j);
int pBestD = i.getBestPosition().at(j);
int gBestD = swarm.getBestPosition().at(j);
int randomParticleNumber = getRandomIntegerValue(0, 1);
int randomGlobalNumber = getRandomIntegerValue(0, 1);
double newVelocityD = parameters.getInertiaWeight() * currentVelocityD +
parameters.getConstant1() * randomParticleNumber * (pBestD - currentPositionD) +
parameters.getConstant2() * randomGlobalNumber * (gBestD - currentPositionD);
if (newVelocityD > parameters.getVMax())
newVelocityD = parameters.getVMax();
else if (newVelocityD < -parameters.getVMax())
newVelocityD = -parameters.getVMax();
int newPositionD = updateStrategy->updatePosition(currentPositionD, newVelocityD);
newVelocity.push_back(newVelocityD);
newPosition.push_back(newPositionD);
}
i.setVelocity(newVelocity);
i.setPosition(newPosition);
int pBestTmp = calculateProfit(i.getPosition());
pBestTmp -= calculatePenalty(i.getPosition(), pBestTmp);
// Update pBest and gBest position/solution
if (pBestTmp > i.getBestValue()) {
i.setBestPositionAndValue(i.getPosition(), pBestTmp);
if (pBestTmp > swarm.getBestValue())
swarm.setBestPositionAndValue(i.getPosition(), pBestTmp);
}
}
}
std::pair<Position,Velocity> ProjectedKinematics::gsAccel(const Position& so, const Velocity& vo, double t, double a) {
Vect3 s3 = so.point();
if (so.isLatLon()) {
s3 = Projection::createProjection(so.lla().zeroAlt()).project(so);
}
Vect3 pres = Kinematics::gsAccelPos(s3,vo,t,a);
Velocity vres = Velocity::mkTrkGsVs(vo.trk(),vo.gs()+a*t,vo.vs());
if (so.isLatLon()) {
return Projection::createProjection(so.lla().zeroAlt()).inverse(pres,vres,true);
} else {
return std::pair<Position,Velocity>(Position(pres), vres);
}
}
/**
* Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
* @param s0 starting point of line 1
* @param v0 direction vector for line 1
* @param s1 starting point of line 2
* @param v1 direction vector of line 2
* @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
* If the lines are parallel, this returns the pair (0,NaN).
*/
double VectFuns::timeOfIntersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
Vect2 so = so3.vect2();
Vect2 vo = vo3.vect2();
Vect2 si = si3.vect2();
Vect2 vi = vi3.vect2();
Vect2 ds = si.Sub(so);
if (vo.det(vi) == 0) {
//f.pln(" $$$ intersection: lines are parallel");
return NaN;
}
double tt = ds.det(vi)/vo.det(vi);
//f.pln(" $$$ intersection: tt = "+tt);
return tt;
}
bool KinematicIntegerBands::cd_future_traj(Detection3D* det, double B, double T, bool trajdir, double t,
const TrafficState& ownship, const TrafficState& ac) const {
if (t > T || B > T) return false;
std::pair<Vect3,Velocity> sovot = trajectory(ownship,t,trajdir);
Vect3 sot = sovot.first;
Velocity vot = sovot.second;
Vect3 si = ac.get_s();
Velocity vi = ac.get_v();
Vect3 sit = vi.ScalAdd(t,si);
if (B > t) {
return conflict(det, sot, vot, sit, vi, B-t, T-t);
}
return conflict(det, sot, vot, sit, vi, 0, T-t);
}
void StateWriter::writeState(const std::string& name, double time, const Position& p, const Velocity& v) {
if (first_line) {
latlon = p.isLatLon();
output.setOutputUnits(display_units);
// Comments and parameters are handled by SeparatedOutput
output.addHeading("name", "unitless");
if (display_time) {
output.addHeading("time", "s");
}
if (latlon) {
output.addHeading("lat", "deg");
output.addHeading("lon", "deg");
output.addHeading("alt", "ft");
} else {
output.addHeading("sx", "NM");
output.addHeading("sy", "NM");
output.addHeading("sz", "ft");
}
if (velocity) {
if (trkgsvs) {
output.addHeading("trk", "deg");
output.addHeading("gs", "knot");
output.addHeading("vs", "fpm");
} else {
output.addHeading("vx", "knot");
output.addHeading("vy", "knot");
output.addHeading("vz", "fpm");
}
}
first_line = false;
}
output.addColumn(name);
if (display_time) {
output.addColumn(FmPrecision(time,precision));
}
output.addColumn(p.toStringList(precision));
if (velocity) {
if (trkgsvs) {
output.addColumn(v.toStringList(precision));
} else {
output.addColumn(v.toStringXYZList(precision));
}
}
lines++;
output.writeLine();
}
XYZ VesselWorldVelocity(Plugin const* const plugin,
char const* vessel_guid,
XYZ const parent_world_velocity,
double const parent_rotation_period) {
Velocity<World> result = CHECK_NOTNULL(plugin)->VesselWorldVelocity(
vessel_guid,
Velocity<World>({parent_world_velocity.x * Metre / Second,
parent_world_velocity.y * Metre / Second,
parent_world_velocity.z * Metre / Second}),
parent_rotation_period * Second);
R3Element<Speed> const& coordinates = result.coordinates();
return XYZ{coordinates.x / (Metre / Second),
coordinates.y / (Metre / Second),
coordinates.z / (Metre / Second)};
}
bool KinematicIntegerBands::any_los_aircraft(Detection3D* det, bool trajdir, double tsk,
const TrafficState& ownship, const std::vector<TrafficState>& traffic) const {
for (TrafficState::nat i=0; i < traffic.size(); ++i) {
TrafficState ac = traffic[i];
std::pair<Vect3,Velocity> sovot = trajectory(ownship,tsk,trajdir);
Vect3 sot = sovot.first;
Velocity vot = sovot.second;
Vect3 si = ac.get_s();
Velocity vi = ac.get_v();
Vect3 sit = vi.ScalAdd(tsk,si);
if (det->violation(sot, vot, sit, vi))
return true;
}
return false;
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnOmegaAlt(const LatLonAlt& so, const Velocity& vo, double t, double omega) {
double currentTrk = vo.trk();
double perpTrk;
if (omega > 0.0) {
perpTrk = currentTrk+M_PI/2;
} else {
perpTrk = currentTrk-M_PI/2;
}
double radius = turnRadiusByRate(vo.gs(), omega);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
//f.pln("center="+center);
LatLonAlt sn = GreatCircle::small_circle_rotation(so,center,omega*t).mkAlt(so.alt()+vo.z*t);
double finalPerpTrk = GreatCircle::initial_course(sn,center);
double nTrk = finalPerpTrk - M_PI/2 * Util::sign(omega);
Velocity vn = vo.mkTrk(nTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turn(LatLonAlt s0, Velocity v0, double t, double R, bool turnRight) {
if (Util::almost_equals(R,0))
return std::pair<LatLonAlt,Velocity>(s0,v0);
int dir = -1;
if (turnRight) dir = 1;
double omega = dir*v0.gs()/R;
return turnOmega(s0,v0,t,omega);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnOmega(const LatLonAlt& so, const Velocity& vo, double t, double omega) {
double currentTrk = vo.trk();
double perpTrk;
if (omega > 0) perpTrk = currentTrk+M_PI/2;
else perpTrk = currentTrk-M_PI/2;
double radius = Kinematics::turnRadiusByRate(vo.gs(), omega);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
double alpha = omega*t;
double vFinalTrk = GreatCircle::initial_course(center,so);
double nTrk = vFinalTrk + alpha;
LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, radius);
sn = sn.mkAlt(so.alt() + vo.z*t);
//double finalTrk = currentTrk+theta;
double final_course = GreatCircle::final_course(center,sn); // TODO: THIS IS PROBABLY BETTER
double finalTrk = final_course + Util::sign(omega)*M_PI/2;
Velocity vn = vo.mkTrk(finalTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
bool Swarm::move_all_slowly(){
bool best_changed = false;
for(int i = 0; i < this->particles.size(); i++){
Velocity v;
int a = rand() % this->nodes.size();
int b = rand() % this->nodes.size();
v.add_transposition(a,b);
this->particles[i].velocity = v;
double val = this->particles[i].move();
if( val < this->best_value ){
this->best_value = val;
this->best_position = this->particles[i].position;
best_changed = true;
}
}
return best_changed;
}
/**
* Given two points on a turn and the velocity (direction) at the first point, determine the direction for the shortest turn going through the second point,
* returning true if that relative direction is to the right
*/
bool ProjectedKinematics::clockwise(Position s1, Velocity v1, Position s2) {
double trk1 = v1.trk();
double trk2;
if (s1.isLatLon()) {
trk2 = GreatCircle::velocity_initial(s1.lla(), s2.lla(), 1).trk();
} else {
trk2 = s2.point().Sub(s1.point()).vect2().track();
}
return Util::clockwise(trk1, trk2);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnByDist(const LatLonAlt& so, const Velocity& vo, double signedRadius, double d) {
double currentTrk = vo.trk();
double perpTrk;
if (signedRadius > 0) perpTrk = currentTrk+M_PI/2;
else perpTrk = currentTrk-M_PI/2;
double radius = std::abs(signedRadius);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
double alpha = d/signedRadius;
double vFinalTrk = GreatCircle::initial_course(center,so);
double nTrk = vFinalTrk + alpha;
LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, radius);
double t = d/vo.gs();
sn = sn.mkAlt(so.alt() + vo.z*t);
double final_course = GreatCircle::final_course(center,sn);
//double finalTrk = currentTrk+alpha;
double finalTrk = final_course + Util::sign(d)*M_PI/2;
Velocity vn = vo.mkTrk(finalTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
/**
* Computes 2D intersection point of two lines, but also finds z component (projected by time from line 1)
* @param s0 starting point of line 1
* @param v0 direction vector for line 1
* @param s1 starting point of line 2
* @param v1 direction vector of line 2
* @return Pair (2-dimensional point of intersection with 3D projection, relative time of intersection, relative to the so3)
* If the lines are parallel, this returns the pair (0,NaN).
*/
std::pair<Vect3,double> VectFuns::intersection(const Vect3& so3, const Velocity& vo3, const Vect3& si3, const Velocity& vi3) {
Vect2 so = so3.vect2();
Vect2 vo = vo3.vect2();
Vect2 si = si3.vect2();
Vect2 vi = vi3.vect2();
Vect2 ds = si.Sub(so);
if (vo.det(vi) == 0) {
//f.pln(" $$$ intersection: lines are parallel");
return std::pair<Vect3,double>(Vect3::ZERO(), NaN);
}
double tt = ds.det(vi)/vo.det(vi);
//f.pln(" $$$ intersection: tt = "+tt);
Vect3 intersec = so3.Add(vo3.Scal(tt));
double nZ = intersec.z;
double maxZ = Util::max(so3.z,si3.z);
double minZ = Util::min(so3.z,si3.z);
if (nZ > maxZ) nZ = maxZ;
if (nZ < minZ) nZ = minZ;
return std::pair<Vect3,double>(intersec.mkZ(nZ),tt);
}
std::pair<LatLonAlt,Velocity> KinematicsLatLon::turnRadius(const LatLonAlt& so, const Velocity& vo, double t, double signedRadius) {
double currentTrk = vo.trk();
double dir = Util::sign(signedRadius);
double perpTrk = currentTrk+dir*M_PI/2;
double radius = std::abs(signedRadius);
LatLonAlt center = GreatCircle::linear_initial(so, perpTrk, radius);
double pathDist = vo.gs()*t;
double theta = pathDist/radius;
//TODO: theta = pathDist/radius, assumes radius is a chord radius, when it is actually a great circle radius.
// for small distances the difference is not that big, but still...
//Note: The other problem is that this assumes a constant speed and constant ground speed through the turn.
// this may or may not be true.
double vFinalTrk = GreatCircle::initial_course(center,so);
double nTrk = vFinalTrk + dir*theta;
LatLonAlt sn = GreatCircle::linear_initial(center, nTrk, radius);
sn = sn.mkAlt(so.alt() + vo.z*t);
double final_course = GreatCircle::final_course(center,sn);
double finalTrk = final_course + dir*M_PI/2;
Velocity vn = vo.mkTrk(finalTrk);
return std::pair<LatLonAlt,Velocity>(sn,vn);
}
void SystemMovement::update(sf::Time dt) {
for (auto entity : movement) {
Velocity* velocity = entity->Get<Velocity*>("Velocity");
if(velocity->isFreeze()){
continue;
}
velocity->adaptVelocity();
Position* position = entity->Get<Position*>("Position");
velocity->updateVelocity(dt, *position);
position->getDirection();
}
// for(auto entity: movement){
// Velocity* velocity = entity->Get<Velocity*>("Velocity");
// velocity->adaptVelocity();
//
// Position* position = entity->Get<Position*>("Position");
// velocity->simulateUpdateVelocity(dt, *position);
// }
}
