LossData WCV_TAUMOD::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
double time_in = T;
double time_out = 0;
double sqs = s.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(table.getDTHR());
double a = v.sqv();
double b = 2*sdotv+table.getTTHR()*v.sqv();
double c = sqs+table.getTTHR()*sdotv-sqD;
if (Util::almost_equals(a,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
time_in = 0;
time_out = T;
return LossData(time_in,time_out);
}
if (sqs <= sqD) {
time_in = 0;
time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
double discr = Util::sq(b)-4*a*c;
if (sdotv >= 0 || discr < 0)
return LossData(time_in,time_out);
double t = (-b - std::sqrt(discr))/(2*a);
if (Horizontal::Delta(s, v,table.getDTHR()) >= 0 && t <= T) {
time_in = std::max(0.0,t);
time_out = std::min(T, Horizontal::Theta_D(s,v,1,table.getDTHR()));
}
return LossData(time_in,time_out);
}
// Compute modified tau
double TCAS2D::tau_mod(double DMOD, const Vect2& s, const Vect2& v) {
double sdotv = s.dot(v);
if (Util::almost_equals(sdotv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
return 0;
return (Util::sq(DMOD)-s.sqv())/sdotv;
}
LossData WCV_TEP::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
double time_in = T;
double time_out = 0;
double sqs = s.sqv();
double sqv = v.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(table.getDTHR());
if (Util::almost_equals(sqv,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
time_in = 0;
time_out = T;
return LossData(time_in,time_out);
}
if (Util::almost_equals(sqv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
return LossData(time_in,time_out);
if (sqs <= sqD) {
time_in = 0;
time_out = Util::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
if (sdotv > 0 || Horizontal::Delta(s,v,table.getDTHR()) < 0)
return LossData(time_in,time_out);
double tep = Horizontal::Theta_D(s,v,-1,table.getDTHR());
if (tep-table.getTTHR() > T)
return LossData(time_in,time_out);
time_in = Util::max(0.0,tep-table.getTTHR());
time_out = Util::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
Vect2 AziEquiProjection::project2(const LatLonAlt& lla) const {
Vect2 p = sphere_to_plane(ref, spherical2xyz(lla.lat(),lla.lon()));
if (p.norm() <= 0.0) {
return Vect2::ZERO;
} else {
return p.Scal(GreatCircle::distance(lla, llaRef)/p.norm());
}
}
double WCV_TEP::horizontal_tvar(const Vect2& s, const Vect2& v) const {
// Time variable is Modified Tau
double TEP = -1;
double sdotv = s.dot(v);
if (sdotv < 0)
return (Util::sq(table.getDTHR())-s.sqv())/sdotv;
return TEP;
}
Vect2 VectFuns::closestPoint(const Vect2& a, const Vect2& b, const Vect2& so) {
// translate a to origin, then project so onto the line defined by ab, then translate back to a
Vect2 ab = b.Sub(a);
return ab.Scal(so.Sub(a).dot(ab)/ab.dot(ab)).Add(a);
// if (collinear(a,b,so)) return so;
// Vect2 v = a.Sub(b).PerpL().Hat(); // perpendicular vector to line
// Vect2 s2 = so.AddScal(100, v);
// Vect2 cp = intersection(so,s2,100,a,b).first;
// return cp;
}
double TCAS2D::time_of_min_tau(double DMOD, double B, double T, const Vect2& s, const Vect2& v) {
if (v.ScalAdd(B,s).dot(v) >= 0)
return B;
double d = Horizontal::Delta(s,v,DMOD);
double rr = 0;
if (d < 0)
rr = 2*std::sqrt(-d) / v.sqv();
if (v.ScalAdd(T,s).dot(v) < 0)
return T;
return nominal_tau(B,T,s,v,rr);
}
/**
* Return actual time of closest point approach (return negative infinity if parallel)
*/
double Vect2::actual_tcpa (const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi){
double rtn;
Vect2 s = so - si;
Vect2 v = vo - vi;
double nv = v.norm();
if (nv > 0) {
rtn = -s.dot(v)/(nv*nv);
} else {
rtn = NINFINITY;;
}
return rtn;
}
Vect2 VectFuns::closestPointOnSegment(const Vect2& a, const Vect2& b, const Vect2& so) {
Vect2 i = closestPoint(a,b,so);
double d1 = a.distance(b);
double d2 = a.distance(i);
double d3 = b.distance(i);
if (d2 <= d1 && d3 <= d1) {
return i;
} else if (d2 < d3) {
return a;
} else {
return b;
}
}
bool LineCheckForBoxOnIsle2(const Vect2 &minVal, const Vect2 &maxVal, const Vect2 &v1, const Vect2 &v2)
{
float tMax = M_INFINITE;
float tMin = -M_INFINITE;
Vect2 rayVect = v2-v1;
float rayLength = rayVect.Len();
Vect2 rayDir = rayVect*(1.0f/rayLength);
Vect2 center = ((maxVal-minVal)*0.5f)+minVal;
Vect2 E = maxVal - center;
Vect2 T = center-v1;
for(int i=0; i<2; i++)
{
float e = T.ptr[i];
float f = rayDir.ptr[i];
float fI = 1.0f/f;
if(fabs(f) > 0.0f)
{
float t1 = (e+E.ptr[i])*fI;
float t2 = (e-E.ptr[i])*fI;
if(t1 > t2)
{
if(t2 > tMin) tMin = t2;
if(t1 < tMax) tMax = t1;
}
else
{
if(t1 > tMin) tMin = t1;
if(t2 < tMax) tMax = t2;
}
if(tMin > tMax) return false;
if(tMax < 0.0f) return false;
}
else if( ((-e - E.ptr[i]) > 0.0f) ||
((-e + E.ptr[i]) < 0.0f) )
{
return FALSE;
}
if(tMin > rayLength) return false;
}
return true;
};
bool WCV_tvar::horizontal_WCV(const Vect2& s, const Vect2& v) const {
if (s.norm() <= table.getDTHR()) return true;
if (Horizontal::dcpa(s,v) <= table.getDTHR()) {
double tvar = horizontal_tvar(s,v);
return 0 <= tvar && tvar <= table.getTTHR();
}
return false;
}
void CreateErrorTexture()
{
LPBYTE textureData = (LPBYTE)Allocate(32*32*4);
msetd(textureData, 0xFF0000FF, 32*32*4);
textures << CreateTexture(32, 32, GS_RGB, textureData, FALSE);
fullSize.Set(32.0f, 32.0f);
Free(textureData);
}
double InitTangentLineY (const Vect2& s, const double D, const int eps) {
// double x;
double y;
double sq_s = s.sqv();
double sq_D = sq(D);
if (Util::almost_equals(sq_s,sq_D)) {
// x = eps*s.y;
y = -eps*s.x;
} else {
Vect2 q = TangentLine::Q(s,D,eps);
// x = q.x;
y = q.y;
if (!q.isZero()) {
// x -= s.x;
y -= s.y;
}
}
return y;
}
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);
}
std::pair<Vect2,double> VectFuns::intersection(const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi) {
Vect2 ds = si.Sub(so);
if (vo.det(vi) == 0) {
//f.pln(" $$$ intersection: lines are parallel");
return std::pair<Vect2,double>(Vect2::ZERO(), NaN);
}
double tt = ds.det(vi)/vo.det(vi);
Vect2 intersec = so.Add(vo.Scal(tt));
return std::pair<Vect2,double>(intersec,tt);
}
Vect2 Vect2::intersect_pt(const Vect2& s0, const Vect2& v0, const Vect2& s1, const Vect2& v1) {
if (Util::almost_equals(v0.det(v1),0.0)) {
//fpln(" $$$$$$$$ ERROR $$$$$$$$$");
return Vect2::INVALID();
} else {
Vect2 delta = s1.Sub(s0);
double ss = delta.det(v1)/v0.det(v1);
return s0.Add(v0.Scal(ss));
}
}
Vect2 TangentLine::Q(const Vect2& s, const double D, const int eps) {
double sq_s = s.sqv();
double sq_D = sq(D);
double delta = sq_s -sq_D;
if (delta >= 0) {
double alpha = sq_D/sq_s;
double beta = D*sqrt_safe(delta)/sq_s;
return Vect2(alpha*s.x+eps*beta*s.y,
alpha*s.y-eps*beta*s.x);
}
return Vect2();
}
/**
* Return distance at time of closest point of approach
**/
double Vect2::dcpa(const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi) {
double t = tcpa(so,vo,si,vi);
Vect2 s = so.Sub(si);
Vect2 v = vo.Sub(vi);
Vect2 st = s.AddScal(t,v);
return st.norm();
}
void TCAS2D::RA2D_interval(double DMOD, double Tau, double B, double T, Vect2 s, Vect2 vo, Vect2 vi) {
t_in = B;
t_out = T;
Vect2 v = vo.Sub(vi);
double sqs = s.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(DMOD);
if (vo.almostEquals(vi) && sqs <= sqD)
return;
double sqv = v.sqv();
if (sqs <= sqD) {
t_out = Util::root2b(sqv,sdotv,sqs-sqD,1);
return;
}
double b = 2*sdotv+Tau*sqv;
double c = sqs+Tau*sdotv-sqD;
if (sdotv >= 0 || Util::discr(sqv,b,c) < 0) {
t_in = T+1;
t_out = 0;
return;
}
t_in = Util::root(sqv,b,c,-1);
if (Horizontal::Delta(s,v,DMOD) >= 0)
t_out = Horizontal::Theta_D(s,v,1,DMOD);
else
t_out = Util::root(sqv,b,c,1);
}
LossData WCV_TCPA::horizontal_WCV_interval(double T, const Vect2& s, const Vect2& v) const {
double time_in = T;
double time_out = 0;
double sqs = s.sqv();
double sqv = v.sqv();
double sdotv = s.dot(v);
double sqD = Util::sq(table.getDTHR());
if (Util::almost_equals(sqv,0) && sqs <= sqD) { // [CAM] Changed from == to almost_equals to mitigate numerical problems
time_in = 0;
time_out = T;
return LossData(time_in,time_out);
}
if (Util::almost_equals(sqv,0)) // [CAM] Changed from == to almost_equals to mitigate numerical problems
return LossData(time_in,time_out);
if (sqs <= sqD) {
time_in = 0;
time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
if (sdotv > 0)
return LossData(time_in,time_out);
double tcpa = Horizontal::tcpa(s,v);
if (v.ScalAdd(tcpa, s).norm() > table.getDTHR())
return LossData(time_in,time_out);
double Delta = Horizontal::Delta(s,v,table.getDTHR());
if (Delta < 0 && tcpa - table.getTTHR() > T)
return LossData(time_in,time_out);
if (Delta < 0) {
time_in = std::max(0.0,tcpa-table.getTTHR());
time_out = std::min(T,tcpa);
return LossData(time_in,time_out);
}
double tmin = std::min(Horizontal::Theta_D(s,v,-1,table.getDTHR()),tcpa-table.getTTHR());
if (tmin > T)
return LossData(time_in,time_out);
time_in = std::max(0.0,tmin);
time_out = std::min(T,Horizontal::Theta_D(s,v,1,table.getDTHR()));
return LossData(time_in,time_out);
}
inline Vect2 Convert3DTo2D(Vect &p, DWORD axis, BOOL bNegative)
{
Vect2 val;
switch(axis)
{
case 0:
val.Set(p.z, p.y);
break;
case 1:
val.Set(p.x, p.z);
break;
case 2:
val.Set(-p.x, p.y);
break;
}
if(bNegative)
val.x = -val.x;
return val;
}
/**
* 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;
}
/**
* Return time to closest point approach
* if time is negative or velocities are parallel returns 0
*/
double Vect2::tcpa (const Vect2& so, const Vect2& vo, const Vect2& si, const Vect2& vi){
double t;
Vect2 s = so.Sub(si);
Vect2 v = vo.Sub(vi);
double nv = v.sqv();
if (nv > 0)
t = std::max(0.0,-s.dot(v)/nv);
else
t = 0;
return t;
}
/**
* 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);
}
bool TCAS2D::min_tau_undef(double DMOD, double B, double T, Vect2 s, Vect2 v) {
return Horizontal::Delta(s,v,DMOD) >= 0 &&
v.ScalAdd(B,s).dot(v) < 0 && v.ScalAdd(T,s).dot(v) >= 0;
}
double TCAS2D::nominal_tau(double B, double T, const Vect2& s, const Vect2& v, double rr) {
if (v.isZero())
return B;
return std::max(B,std::min(T,-s.dot(v) / v.sqv()-rr/2));
}
bool TCAS2D::horizontal_RA_at(double DMOD, double Tau, const Vect2& s, const Vect2& v, double t) {
const Vect2& sat = v.ScalAdd(t,s);
return horizontal_RA(DMOD,Tau,sat,v);
}
bool TCAS2D::horizontal_RA(double DMOD, double Tau, const Vect2& s, const Vect2& v) {
if (s.dot(v) >= 0) return s.norm() <= DMOD;
else return s.norm() <= DMOD || tau_mod(DMOD,s,v) <= Tau;
}
BOOL Line2::LinesIntersect(const Line2 &collider) const
{
//this looks more complex than it really is. I'm just an optimization freak.
//...okay, this probably isn't the kind of function that's called every frame or something.
//but I'm compulsive about optimization! ..okay, I'll make it cleaner one of these days.
Vect2 vec = (A-B);
float len = vec.Len();
Vect2 norm = (vec/len);
Vect2 cross = norm.GetCross();
float dist = cross.Dot(A);
float fADist = collider.A.Dot(cross)-dist;
float fBDist = collider.B.Dot(cross)-dist;
BOOL bAAbove = (fADist > -EPSILON);
BOOL bBAbove = (fBDist > -EPSILON);
BOOL bFoundZeroLine1 = FALSE;
if( (fabs(fADist) < EPSILON) ||
(fabs(fBDist) < EPSILON) )
{
bFoundZeroLine1 = TRUE;
}
if(bAAbove == bBAbove)
return FALSE;
vec = (collider.A-collider.B);
len = vec.Len();
norm = (vec/len);
cross = norm.GetCross();
dist = cross.Dot(collider.A);
fADist = A.Dot(cross)-dist;
fBDist = B.Dot(cross)-dist;
bAAbove = (fADist > -EPSILON);
bBAbove = (fBDist > -EPSILON);
if(fabs(fADist) < EPSILON)
bAAbove = bBAbove;
if(fabs(fBDist) < EPSILON)
bBAbove = bAAbove;
BOOL bFoundZeroLine2 = FALSE;
if( (fabs(fADist) < EPSILON) ||
(fabs(fBDist) < EPSILON) )
{
bFoundZeroLine2 = TRUE;
}
if(bAAbove == bBAbove)
return FALSE;
if(bFoundZeroLine1 && bFoundZeroLine2)
return FALSE;
return TRUE;
}
void D3D10System::DrawSpriteEx(Texture *texture, DWORD color, float x, float y, float x2, float y2, float u, float v, float u2, float v2)
{
if(!curPixelShader)
return;
if(!texture)
{
AppWarning(TEXT("Trying to draw a sprite with a NULL texture"));
return;
}
HANDLE hColor = curPixelShader->GetParameterByName(TEXT("outputColor"));
if(hColor)
curPixelShader->SetColor(hColor, color);
//------------------------------
// crop positional values
Vect2 totalSize = Vect2(x2-x, y2-y);
Vect2 invMult = Vect2(totalSize.x < 0.0f ? -1.0f : 1.0f, totalSize.y < 0.0f ? -1.0f : 1.0f);
totalSize.Abs();
x += curCropping[0] * invMult.x;
y += curCropping[1] * invMult.y;
x2 -= curCropping[2] * invMult.x;
y2 -= curCropping[3] * invMult.y;
//------------------------------
// crop texture coordinate values
float cropMult[4];
cropMult[0] = curCropping[0]/totalSize.x;
cropMult[1] = curCropping[1]/totalSize.y;
cropMult[2] = curCropping[2]/totalSize.x;
cropMult[3] = curCropping[3]/totalSize.y;
Vect2 totalUVSize = Vect2(u2-u, v2-v);
u += cropMult[0] * totalUVSize.x;
v += cropMult[1] * totalUVSize.y;
u2 -= cropMult[2] * totalUVSize.x;
v2 -= cropMult[3] * totalUVSize.y;
//------------------------------
// draw
VBData *data = spriteVertexBuffer->GetData();
data->VertList[0].Set(x, y, 0.0f);
data->VertList[1].Set(x, y2, 0.0f);
data->VertList[2].Set(x2, y, 0.0f);
data->VertList[3].Set(x2, y2, 0.0f);
List<UVCoord> &coords = data->UVList[0];
coords[0].Set(u, v);
coords[1].Set(u, v2);
coords[2].Set(u2, v);
coords[3].Set(u2, v2);
spriteVertexBuffer->FlushBuffers();
LoadVertexBuffer(spriteVertexBuffer);
LoadTexture(texture);
Draw(GS_TRIANGLESTRIP);
}