void BodyRigid::forceAndTorqueAccum(Vect3 const &f, bool forceInLocal, Vect3 const &forceAppLocal, Vect3 const &t, bool torqueInLocal)
{
Vect3 force; Vect3 torque;
if(forceInLocal)
{
force = m_q*f;
torque = forceAppLocal.cross(f);
}
else
{
force = f;
torque = forceAppLocal.cross(m_q.inverse()*f);
}
if(torqueInLocal)
{
torque += t;
}
else
{
torque += m_q.inverse()*t;
}
m_appliedForce += force;
m_appliedTorque += torque;
}
// 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 CollisionSphere::genContactsS(const CollisionSphere& s, std::vector<std::shared_ptr<IContact>>& o) const
{
// Get distance between two objects, with this one at the origin...
Vect3 distance = s.getPos() - this->getPos();
// Spheres are touching if the magnitude of the distance is less than
// the sum of the radii
// Early out if this is not the case.
if (distance.squareMagnitude() > ((this->getRadius() + s.getRadius()) * (this->getRadius() + s.getRadius())))
{
return;
}
else
{
// Generate contacts!
// Contact is midpoint in world space of intersection,
// normal is direction of intersection.
Vect3Normal directionNormal = (s.getPos() - this->getPos());
float contactMagnitude = this->getRadius() + s.getRadius() - distance.magnitude();
// From the other object, go to the edge of the sphere.
// Subtract that from the edge of this sphere.
// now multiply the whole thing by 1/2, and add the edge of this sphere.
// That's the midpoint of the intersected space of the two spheres.
Vect3 contactPoint = ((s.getPos() - directionNormal * s.getRadius())
- (this->getPos() + directionNormal * this->getRadius()))
* 0.5f
+ (this->getPos() + directionNormal * this->getRadius());
o.push_back(summonDemons(contactPoint, directionNormal * -1.f, contactMagnitude, s.getAttachedObjectPtr(), _attachedObject));
o.push_back(summonDemons(contactPoint, directionNormal, contactMagnitude, this->getAttachedObjectPtr(), s.getAttachedObjectPtr()));
}
}
void Force2BodySpringDamp::apply(double t)
{
// pt1 on body body 1 & pt2 on body 2
Vect3 pt1Pos = m_body1->getPosition(m_body1Offset);
Vect3 pt1Vel = m_body1->getVelocityGlobal(m_body1Offset);
Vect3 pt2Pos = m_body2->getPosition(m_body2Offset);
Vect3 pt2Vel = m_body2->getVelocityGlobal(m_body2Offset);
// distance between 2 points in global
Vect3 dist = pt2Pos-pt1Pos;
// Relative Velocity
Vect3 revVel = pt2Vel - pt1Vel;
// just to have meaningfull names
Vect3 normalForceVect = dist;
//normailze normalForceVector in place and get magnitude
double dist_mag = magnitude(normalForceVect);
normalForceVect.normalize();
double springForceMag = (dist_mag-m_fl)*m_k;
double velAlongSpring = normalForceVect.dot(revVel);
double dampForceMag = velAlongSpring * m_c;
double totalForceMag = springForceMag + dampForceMag;
Vect3 totalForceVect = normalForceVect * totalForceMag;
m_body1->forceAccum(totalForceVect,false,m_body1Offset);
m_body2->forceAccum(-totalForceVect,false,m_body2Offset);
}
void TIME_STEPPER::time_loop() {
if (first_step) {
REAL OmRMax = 0.0, MaxRadius = 0.0;
if (globalSystem->useBodies) {
for (int i = 0; i < BODY::AllBodyFaces.size(); ++i) {
Vect3 Pos = BODY::AllBodyFaces[i]->CollocationPoint - BODY::AllBodyFaces[i]->Owner->CG;
// Get point kinematic velocity - rotational part first
Vect3 Vrot = BODY::AllBodyFaces[i]->Owner->BodyRates.Cross(Pos);
// Add to translational velocity....
Vect3 Vkin = BODY::AllBodyFaces[i]->Owner->Velocity + Vrot;
OmRMax = max(Vkin.Mag(), OmRMax);
MaxRadius = max(MaxRadius, Pos.Mag());
}
}
dt = globalSystem->dtInit;// = cfl_lim/OmRMax;
dt_prev = cfl_lim/OmRMax;
if (globalSystem->useBodies) {
BODY::BodySubStep(globalSystem->dtInit, globalSystem->NumSubSteps);
globalSystem->PutWakesInTree();
}
globalOctree->Reset();
globalOctree->GetVels();
// if (globalSystem->useFMM) {
//#pragma omp parallel for
// for (int i = 0; i < FVMCell::AllCells.size(); ++i)
// for (int j = 0; j < FVMCell::AllCells.size(); ++j)
// FVMCell::AllCells[i]->Velocity += UTIL::globalDirectVel(FVMCell::AllCells[j]->Position - FVMCell::AllCells[i]->Position, FVMCell::AllCells[j]->Omega);
// }
first_step = false;
} else {
#ifdef TIME_STEPS
long unsigned int t1 = ticks();
#endif
TimeAdvance();
// Produce Output
if (globalTimeStepper->dump_next) {
globalSystem->WriteData();
}
#ifdef TIME_STEPS
long unsigned int t13 = ticks();
stringstream tmp;
tmp << "Total....................: " << double(t13 - t1) / 1000.0 << endl;
globalIO->step_data += tmp.str();
#endif
// Display Status
globalIO->stat_step();
}
}
void BodyRigid::forceAccumGlobal(Vect3 const &globalForce, Vect3 const &globalPnt)
{
Vect3 fLocal = m_q.inverse() * globalForce;
Vect3 pntLocal = m_q.inverse() * (globalPnt-m_pos);
Vect3 tLocal = pntLocal.cross(fLocal);
//std::cout << "dfadsfads" << std::endl;
m_appliedForce += globalForce;
m_appliedTorque += tLocal;
}
// time of closest approach
double VectFuns::tau(const Vect3& s, const Vect3& vo, const Vect3& vi) {
double rtn;
Vect3 v = vo.Sub(vi);
double nv = v.norm();
if (Util::almost_equals(nv,0.0)) {
rtn = std::numeric_limits<double>::max(); // pseudo infinity
} else
rtn = -s.dot(v)/(nv*nv);
return rtn;
}// tau
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);
}
void BodyRigid::setMassInertia(Vect3 const &row0,Vect3 const &row1,Vect3 const &row2)
{
m_inertia(0,0)=row0.x(); m_inertia(0,1)=row0.y(); m_inertia(0,2)=row0.z();
m_inertia(1,0)=row1.x(); m_inertia(1,1)=row1.y(); m_inertia(1,2)=row1.z();
m_inertia(2,0)=row2.x(); m_inertia(2,1)=row2.y(); m_inertia(2,2)=row2.z();
cout << m_inertia << std::endl;
}
void Quat::invXform(Vect3 &v) const
{
Vect3 *u, uv, uuv;
u = (Vect3 *) &x_;
uv.cross(*u, v);
uuv.cross(*u, uv);
uv.scale(2.0 * -s_);
uuv.scale(2.0);
v.add(uv);
v.add(uuv);
}
bool Target_Icosahedron::Check(real x, real y, real z)
{
const int indd[20] = { 0, 0, 0, 0, 0, 5, 6, 7, 8, 8, 9, 9, 9, 10, 10, 11, 11, 11, 11, 11 };
Vect3<real> tmp;
bool res = true;
for(int i=0; i<numNormals; ++i)
{
tmp.Set(x, y, z);
res = res && ((normals[i].Scalar(tmp - vertices[indd[i]])) <= (real)0.);
}
return res;
}
/**
* Returns values indicating whether the ownship state will pass in front of or behind the intruder (from a horizontal perspective)
* @param so ownship position
* @param vo ownship velocity
* @param si intruder position
* @param vi intruder velocity
* @return 1 if ownship will pass in front (or collide, from a horizontal sense), -1 if ownship will pass behind, 0 if divergent or parallel
*/
int VectFuns::passingDirection(const Vect3& so, const Velocity& vo, const Vect3& si, const Velocity& vi) {
double toi = timeOfIntersection(so,vo,si,vi);
double tii = timeOfIntersection(si,vi,so,vo); // these values may have opposite sign!
//fpln("toi="+toi);
//fpln("int = "+ intersection(so,vo,si,vi));
if (ISNAN(toi) || toi < 0 || tii < 0) return 0;
Vect3 so3 = so.linear(vo, toi);
Vect3 si3 = si.linear(vi, toi);
//fpln("so3="+so3);
//fpln("si3="+si3);
if (behind(so3.vect2(), si3.vect2(), vi.vect2())) return -1;
return 1;
}
Vect3 VectFuns::closestPointOnSegment(const Vect3& a, const Vect3& b, const Vect3& so) {
Vect3 i = closestPoint(a,b,so);
double d1 = a.distanceH(b);
double d2 = a.distanceH(i);
double d3 = b.distanceH(i);
if (d2 <= d1 && d3 <= d1) {
return i;
} else if (d2 < d3) {
return a;
} else {
return b;
}
}
bool Target_Dodecahedron::Check(real x, real y, real z)
{
const int indd[12] = { 0, 0, 1, 2, 3, 4, 15, 16, 17, 18, 19, 15 };
Vect3<real> tmp;
bool res = true;
for(int i=0; i<numNormals; ++i)
{
tmp.Set(x, y, z);
res = res && ((normals[i].Scalar(tmp - vertices[indd[i]])) <= (real)0.);
}
return res;
}
void Quat::xform(const Vect3 &v, Vect3 &xv) const
{
Vect3 *u, uv, uuv;
u = (Vect3 *) &x_;
uv.cross(*u, v);
uuv.cross(*u, uv);
uv.scale(2.0 * s_);
uuv.scale(2.0);
xv.add(v, uv);
xv.add(uuv);
}
/**
* 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;
}
static double dpc(const Vect3& m, const Vect3 *pts, const Triangle& cell, Vect3& alphas, int nb, int* idx, bool& inside)
{
if(nb == 1) {
alphas(idx[0]) = 1.0;
return (m - pts[cell[idx[0]]]).norm();
}
// Solves H=sum(alpha_i A_i), sum(alpha_i)=1, et HM.(A_i-A_0)=0
Vect3 A0Ai[3]; // A_i-A_0
for(int i=1; i<nb; i++) {
A0Ai[i] = pts[cell[idx[i]]] - pts[cell[idx[0]]];
}
Vect3 A0M = m - pts[cell[idx[0]]]; // M-A_0
if(nb == 2) {
alphas(idx[1]) = (A0M * A0Ai[1]) / (A0Ai[1] * A0Ai[1]);
alphas(idx[0]) = 1.0 - alphas(idx[1]);
} else if (nb==3) {
// direct inversion (2x2 linear system)
double a00 = A0Ai[1] * A0Ai[1];
double a10 = A0Ai[1] * A0Ai[2];
double a11 = A0Ai[2] * A0Ai[2];
double b0 = A0M * A0Ai[1];
double b1 = A0M * A0Ai[2];
double d = a00 * a11 - a10 * a10;
assert(d != 0);
alphas(idx[1]) = (b0 * a11 - b1 * a10) / d;
alphas(idx[2]) = (a00 * b1 - a10 * b0) / d;
alphas(idx[0]) = 1.0 - alphas(idx[1]) - alphas(idx[2]);
} else {
// 3 unknowns or more -> solve system
// Ax=b with: A(i, j)=A0Ai.AjA0, x=(alpha_1, alpha_2, ...), b=A0M.A0Ai
cerr << "Error : dim>=4 in danielsson !" << endl;
exit(0);
}
// If alpha_i<0 -> brought to 0 and recursion
// NB: also takes care of alpha > 1 because if alpha_i>1 then alpha_j<0 for at least one j
for (int i=0; i<nb; i++) {
if (alphas(idx[i])<0) {
inside = false;
alphas(idx[i]) = 0;
swap(idx[i], idx[nb-1]);
return dpc(m, pts, cell, alphas, nb-1, idx, inside);
}
}
// Sinon: distance HM
Vect3 MH = -A0M;
for (int i=1; i<nb; i++) {
MH = MH + alphas(idx[i]) * A0Ai[i];
}
return MH.norm();
}
/* Solve the following equation on vz:
* sz+t*vz = eps*H,
*
* where t = Theta_D(s,v,epsp).
* eps determines the bottom, i.e.,-1, or top, i.e., 1, circle.
*/
Vertical Vertical::vs_circle(const Vect3& s, const Vect3& vo, const Vect3& vi,
const int eps, const double D, const double H) {
Vect2 s2 = s.vect2();
Vect2 vo2 = vo.vect2();
Vect2 vi2 = vi.vect2();
Vect2 v2 = vo2-vi2;
if (vo2.almostEquals(vi2) && eps == sign(s.z))
return Vertical(vi.z);
else if (Horizontal::Delta(s2,v2,D) > 0)
return vs_only(s,v2,Horizontal::Theta_D(s2,v2,larcfm::Exit,D),eps,D,H).add_this(vi.z);
return NoVerticalSolution;
}
// TODO: Test!
// REALLY TODO: TEST. There definitely is a bug in here.
void CollisionSphere::genContactsB(const CollisionBox& b, std::vector<std::shared_ptr<IContact>>& o) const
{
// Get the sphere position in local coordinates of the box.
Vect3 transformedSpherePosition = b.getTransformMatrix().getInverse() * this->getPos();
// If the sphere is further away than the magnitude of the furthest distance
// a corner can be, early out:
if (transformedSpherePosition.squareMagnitude() > b.getSize().squareMagnitude())
return;
// Otherwise, find the closest point on the cube.
// For any axis, it will be the cube half length on that side if
// the sphere's position is further than the half length. If not, it'll
// just be the component on that side.
Vect3 closestBoxPoint = transformedSpherePosition;
closestBoxPoint._x = CLAMP(closestBoxPoint._x, -b.getSize()._x, b.getSize()._x);
closestBoxPoint._y = CLAMP(closestBoxPoint._y, -b.getSize()._y, b.getSize()._y);
closestBoxPoint._z = CLAMP(closestBoxPoint._z, -b.getSize()._z, b.getSize()._z);
// Now we have the closest point on the box.
// If it's closer than the radius, we have a contact
if ((closestBoxPoint - transformedSpherePosition).squareMagnitude() < this->getRadius() * this->getRadius()
&& ((closestBoxPoint - transformedSpherePosition).squareMagnitude() > 0.f))
{
// Box contact data: Contact is under the surface of the sphere, pointing directly out.
Vect3Normal d = (closestBoxPoint - transformedSpherePosition);
Vect3 collisionPoint_l = transformedSpherePosition + d * _radius;
Vect3 penetration_l = closestBoxPoint - collisionPoint_l;
Vect3 collisionPoint_w = b.getTransformMatrix() * collisionPoint_l;
Vect3 penetration_w = b.getTransformMatrix().transformDirn(penetration_l);
/*Vect3 collisionPoint_l = (Vect3Normal(closestBoxPoint - this->GetPos()) * this->GetRadius()) + this->GetPos();
Vect3 collisionPoint_w = b.GetTransformMatrix() * (closestBoxPoint - transformedSpherePosition);
Vect3 penetration_w = (Vect3Normal(collisionPoint_w - this->GetPos()) * this->GetRadius()) - collisionPoint_w;*/
o.push_back(this->summonDemons(
collisionPoint_w,
penetration_w,
penetration_w.magnitude(),
b.getAttachedObjectPtr(), _attachedObject));
// Sphere contact data: Exact opposite of the box contact.
penetration_w *= -1.f;
o.push_back(this->summonDemons(
collisionPoint_w,
penetration_w,
penetration_w.magnitude(),
this->getAttachedObjectPtr(), b.getAttachedObjectPtr()));
}
}
std::pair<Vect3,double> VectFuns::intersectionAvgZ(const Vect3& so1, const Vect3& so2, double dto, const Vect3& si1, const Vect3& si2) {
Velocity vo3 = Velocity::genVel(so1, so2, dto);
Velocity vi3 = Velocity::genVel(si1, si2, dto); // its ok to use any time here, all times are relative to so
std::pair<Vect3,double> iP = intersection(so1,vo3,si1,vi3);
Vect3 interSec = iP.first;
double do1 = distanceH(so1,interSec);
double do2 = distanceH(so2,interSec);
double alt_o = so1.z;
if (do2 < do1) alt_o = so2.z;
double di1 = distanceH(si1,interSec);
double di2 = distanceH(si2,interSec);
double alt_i = si1.z;
if (di2 < di1) alt_i = si2.z;
double nZ = (alt_o + alt_i)/2.0;
return std::pair<Vect3,double>(interSec.mkZ(nZ),iP.second);
}
void HOATDispatcher::sendExtendedResponse(
CigiOutgoingMsg *outgoing,
HOTRequest *request,
HOTResponse *response, int numResponses )
{
CigiHatHotXRespV3_2 packet;
packet.SetHatHotID( request->id );
packet.SetValid( true );
packet.SetHostFrame( request->hostFrameNumber & 0x0f );
Vect3 delta = response->hitLocation - request->locationMSL;
//FIXME - negative heights must be handled!!!
double hot = delta.mag();
packet.SetHot( hot );
delta = request->location - response->hitLocation;
//FIXME - negative heights must be handled!!!
double hat = delta.mag();
packet.SetHat( hat );
if( terrainMaterialOverride )
packet.SetMaterial( terrainMaterialOverrideCode );
else
packet.SetMaterial( response->materialCode );
// fixme - does this work w/ geocentric?
double heading = 180.0/M_PI * normalize_angle( atan2(response->normal[1], response->normal[0]) );
double pitch = 180.0/M_PI * normalize_angle( atan2(response->normal[1], response->normal[2]) );
// Convert database heading/pitch into geodetic
CoordinateSet lru_pos;
lru_pos.LatX = response->hitLocation[0];
lru_pos.LonY = response->hitLocation[1];
lru_pos.AltZ = response->hitLocation[2];
lru_pos.Yaw = heading;
lru_pos.Pitch = pitch;
lru_pos.Roll = 0.0;
CoordinateSet gd_pos;
coordinateConverter->performReverseConversion( lru_pos, gd_pos );
packet.SetNormAz( normalize_angle( gd_pos.Yaw ) );
packet.SetNormEl( clamp_pitch( gd_pos.Pitch ) );
*outgoing << packet;
}
static Vect3 equator_map_inv(const Vect3& ref, const Vect3& p) {
Vect3 xmult = ref.Hat();
Vect3 ymult = vect3_orthog_toy(ref).Hat();
Vect3 zmult = ref.cross(vect3_orthog_toy(ref)).Hat();
Vect3 xmultInv = Vect3(xmult.x, ymult.x, zmult.x);
Vect3 ymultInv = Vect3(xmult.y, ymult.y, zmult.y);
Vect3 zmultInv = Vect3(xmult.z, ymult.z, zmult.z);
return Vect3(xmultInv.dot(p), ymultInv.dot(p), zmultInv.dot(p));
}
/**
* 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);
}
Vect3 VectFuns::closestPoint(const Vect3& a, const Vect3& b, const Vect3& so) {
if (a.almostEquals(b)) return Vect3::INVALID();
Vect2 c = closestPoint(a.vect2(), b.vect2(), so.vect2());
Vect3 v = b.Sub(a);
double d1 = v.vect2().norm();
double d2 = c.Sub(a.vect2()).norm();
double d3 = c.Sub(b.vect2()).norm();
double f = d2/d1;
if (d3 > d1 && d3 > d2) { // negative direction
f = -f;
}
return a.AddScal(f, v);
// Vect3 v = a.Sub(b).PerpL().Hat2D(); // perpendicular vector to line
// Vect3 s2 = so.AddScal(100, v);
// std::pair<Vect3, double> i = intersectionAvgZ(a,b,100,so,s2);
// // need to fix altitude to be along the a-b line
// return interpolate(a,b,i.second/100.0);
}
Vertical vs_only(const Vect3& s, const Vect2& v, const double t,
const int eps, const double D, const double H) {
Vect2 s2 = s.vect2();
if (eps*s.z < H && s2.sqv() > sq(D) && Horizontal::Delta(s2,v,D) > 0)
return vs_at(s.z,Horizontal::Theta_D(s2,v,larcfm::Entry,D),eps,H);
else if (eps*s.z >= H && t > 0)
return vs_at(s.z,t,eps,H);
return Vertical::NoVerticalSolution;
}
// The user needs to keep track of whether to translate back (i.e. whether original was LatLon())
Velocity AziEquiProjection::inverseVelocity(const Vect3& s, const Velocity& v, bool toLatLon) const {
if (toLatLon) {
double timeStep = 10.0;
Vect3 s2 = s.linear(v,timeStep);
LatLonAlt lla1 = inverse(s);
LatLonAlt lla2 = inverse(s2);
Velocity nv = GreatCircle::velocity_initial(lla1,lla2,timeStep);
return nv;
} else {
return v;
}
}
double VectFuns::distAtTau(const Vect3& s, const Vect3& vo, const Vect3& vi, bool futureOnly) {
double tau = VectFuns::tau(s,vo,vi);
if (tau < 0 && futureOnly)
return s.norm(); // return distance now
else {
Vect3 v = vo.Sub(vi);
Vect3 sAtTau = s.Add(v.Scal(tau));
return sAtTau.norm();
}
}
void OnSolver::Solve(double time, Matrix& FF){
system->SetTime(time);
for(int i=1;i<numbodies;i++)
bodyarray[i]->LocalKinematics();
Vect3 Torque; Torque.Zeros();
Vect3 Force; Force.Zeros();
for(int i=numbodies-1;i>0;i--){
Torque(1)=FF(1,i);
Torque(2)=FF(2,i);
Torque(3)=FF(3,i);
Force(1)=FF(4,i);
Force(2)=FF(5,i);
Force(3)=FF(6,i);
bodyarray[i]->LocalTriangularization(Torque,Force);
}
for(int i=1;i<numbodies;i++){
bodyarray[i]->LocalForwardSubstitution();
}
}
bool KinematicIntegerBands::no_instantaneous_conflict(Detection3D* conflict_det, Detection3D* recovery_det,
double B, double T, double B2, double T2,
bool trajdir, const TrafficState& ownship, const std::vector<TrafficState>& traffic,
const TrafficState& repac,
int epsh, int epsv) {
bool usehcrit = repac.isValid() && epsh != 0;
bool usevcrit = repac.isValid() && epsv != 0;
std::pair<Vect3,Velocity> nsovo = trajectory(ownship,0,trajdir);
Vect3 so = ownship.get_s();
Vect3 vo = ownship.get_v();
Vect3 si = repac.get_s();
Vect3 vi = repac.get_v();
Vect3 nvo = nsovo.second;
Vect3 s = so.Sub(si);
return
(!usehcrit || CriteriaCore::horizontal_new_repulsive_criterion(s,vo,vi,nvo,epsh)) &&
(!usevcrit || CriteriaCore::vertical_new_repulsive_criterion(s,vo,vi,nvo,epsv)) &&
no_conflict(conflict_det,recovery_det,B,T,B2,T2,trajdir,0,ownship,traffic);
}
bool KinematicIntegerBands::vert_repul_at(double tstep, bool trajdir, int k, const TrafficState& ownship,
const TrafficState& repac, int epsv) const {
// repac is not NULL at this point and k >= 0
if (k==0) {
return true;
}
std::pair<Vect3,Velocity> sovo = trajectory(ownship,0,trajdir);
Vect3 so = sovo.first;
Vect3 vo = sovo.second;
Vect3 si = repac.get_s();
Vect3 vi = repac.get_v();
bool rep = true;
if (k==1) {
rep = CriteriaCore::vertical_new_repulsive_criterion(so.Sub(si),vo,vi,linvel(ownship,tstep,trajdir,0),epsv);
}
if (rep) {
std::pair<Vect3,Velocity> sovot = trajectory(ownship,k*tstep,trajdir);
Vect3 sot = sovot.first;
Vect3 vot = sovot.second;
Vect3 sit = vi.ScalAdd(k*tstep,si);
Vect3 st = sot.Sub(sit);
Vect3 vop = linvel(ownship,tstep,trajdir,k-1);
Vect3 vok = linvel(ownship,tstep,trajdir,k);
return CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vot,epsv) &&
CriteriaCore::vertical_new_repulsive_criterion(st,vot,vi,vok,epsv) &&
CriteriaCore::vertical_new_repulsive_criterion(st,vop,vi,vok,epsv);
}
return false;
}
