LLA ECEFtoLLA(ECEF const &ecef, ReferenceEllipsoid const &_ref) { double _lon = atan2(ecef.y(), ecef.x()); double _lat, _alt; double p = sqrt(ecef.x()*ecef.x()+ecef.y()*ecef.y()); double q = _ref.A_B * ecef.z(); double h = 1.0 / sqrt(p*p+q*q); _iterateECEF(_lat, _alt, p, ecef.z(), p*h, q*h, ecef.z(), _ref); return LLA(_lon, _lat, _alt); }
LLA UTMtoLLA(UTM const &utm, ReferenceEllipsoid const &_ref) { // central-meridian scale factor static double k0 = 0.9996; double nu, T, T2, S, C, CP, SP, R, D, D2, M; double mu, phi; double x, y; x = utm.easting() - 500000.0; //remove 500,000 meter offset for longitude y = utm.northing(); if ((utm.designator() - 'N') < 0) { y -= 10000000.0; //remove 10,000,000 meter offset used for southern hemisphere } double lon0 = toRadians(((utm.zone() - 1) * 6.0 - 180.0 + 3.0)); //+3 puts origin in middle of zone M = y / k0; mu = M * _ref.m_f; phi = mu + _ref.m_a*sin(2.0*mu) + _ref.m_b*sin(4.0*mu) + _ref.m_c*sin(6.0*mu); C = cos(phi); S = sin(phi); T = S/C; nu = _ref.A/sqrt(1.0-_ref.e2*S*S); T2 = T * T; CP = C * C * _ref.ep2; SP = 1.0 - S * S * _ref.e2; R = _ref.A * _ref.B2_A2 / (SP*sqrt(SP)); D = x/(nu*k0); D2 = D*D; double lat, lon; lat = phi - (nu*T/R)*(D2*(0.5 - D2*((120.0+90.0*T2+CP*(300.0-120.0*CP)-270.0*_ref.ep2) +(61.0+90.0*T2+298.0*CP+45.0*T2*T2-252.0*_ref.ep2-3.0*CP*CP)*D2)/720.0)); lon = D * ( 1.0 - D2*( (1.0+2.0*T2+CP)/6.0 - ( 5.0 - CP*(2.0 + 3.0*CP) + 8.0*_ref.ep2 + T2*(28.0 + 24.0*T2) )*D2/120.0 ) ) / C; lon += lon0; return LLA(lat, lon, utm.altitude()); }
bool collisionRectSAT(glm::vec4& URA4, glm::vec4& LRA4, glm::vec4& LLA4, glm::vec4& ULA4, glm::vec4& URB4, glm::vec4& LRB4, glm::vec4& LLB4, glm::vec4& ULB4) { glm::vec3 URA(URA4); glm::vec3 LRA(LRA4); glm::vec3 LLA(LLA4); glm::vec3 ULA(ULA4); glm::vec3 URB(URB4); glm::vec3 LRB(LRB4); glm::vec3 LLB(LLB4); glm::vec3 ULB(ULB4); glm::vec3 axis[4]; axis[0].x = URA.x - ULA.x; axis[0].y = URA.y - ULA.y; axis[1].x = URA.x - LRA.x; axis[1].y = URA.y - LRA.y; axis[2].x = ULB.x - LLB.x; axis[2].y = ULB.y - LLB.y; axis[3].x = ULB.x - URB.x; axis[3].y = ULB.y - URB.y; glm::vec3 vertA[] = {URA, LRA, LLA, ULA}; glm::vec3 vertB[] = {URB, LRB, LLB, ULB}; glm::vec3 projA[4]; glm::vec3 projB[4]; std::vector<double> positionA(4); std::vector<double> positionB(4); for (int a = 0; a < 4; a++) { for (int i = 0; i < 4; i++) { double top = vertA[i].x * axis[a].x + vertA[i].y * axis[a].y; double bottom = axis[a].x * axis[a].x + axis[a].y * axis[a].y; double common = top / bottom; projA[i].x = common * axis[a].x; projA[i].y = common * axis[a].y; positionA.at(i) = glm::dot(projA[i], axis[a]); } for (int i = 0; i < 4; i++) { double top = vertB[i].x * axis[a].x + vertB[i].y * axis[a].y; double bottom = axis[a].x * axis[a].x + axis[a].y * axis[a].y; double common = top / bottom; projB[i].x = common * axis[a].x; projB[i].y = common * axis[a].y; positionB.at(i) = glm::dot(projB[i], axis[a]); } double minA = *(std::min_element(positionA.begin(), positionA.end())); double minB = *(std::min_element(positionB.begin(), positionB.end())); double maxA = *(std::max_element(positionA.begin(), positionA.end())); double maxB = *(std::max_element(positionB.begin(), positionB.end())); //no collision on this axis, none at all if (((minB > maxA) || (maxB < minA))) { return false; } } return true; }