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