double BiSector(double InBound, double OutBound) { double result; InBound = Reciprocal(InBound); if(InBound == OutBound) { result = Reciprocal(InBound); } else if (InBound > OutBound) { if( (InBound - OutBound) < 180) { result = Reciprocal((InBound+OutBound)/2); } else { result = (InBound+OutBound)/2; } } else { if( (OutBound - InBound) < 180) { result = Reciprocal((InBound+OutBound)/2); } else { result = (InBound+OutBound)/2; } } return result; }
bool InTurnSector(NMEA_INFO *Basic, DERIVED_INFO *Calculated, const int the_turnpoint) { if(!ValidTaskPoint(the_turnpoint)) return false; switch(SectorType) { case CIRCLE: if(Calculated->WaypointDistance < SectorRadius) return true; break; case SECTOR: case DAe: { if(SectorType==DAe) { // JMW added german rules if (Calculated->WaypointDistance<500) return true; } double AircraftBearing; LockTaskData(); DistanceBearing(WayPointList[Task[the_turnpoint].Index].Latitude, WayPointList[Task[the_turnpoint].Index].Longitude, Basic->Latitude , Basic->Longitude, NULL, &AircraftBearing); AircraftBearing = AircraftBearing - Task[the_turnpoint].Bisector; UnlockTaskData(); while(AircraftBearing<-180) AircraftBearing+= 360; while(AircraftBearing>180) AircraftBearing-= 360; if(AircraftBearing>=-45 && AircraftBearing<=45) { if(SectorType==SECTOR) { if(Calculated->WaypointDistance < SectorRadius) return true; } else { //It's a DAe if(Calculated->WaypointDistance < 10000) return true; // JMW added german rules } } } break; case LINE: { //First check if we simply passed the WayPoint LockTaskData(); if(Calculated->LegDistanceToGo<Task[the_turnpoint].Leg && Calculated->LegDistanceCovered>=Task[the_turnpoint].Leg) { UnlockTaskData(); return true; } //Then check if we passed the bisector bool bisectorOverpassed; if(AngleLimit360(Task[the_turnpoint].InBound-Task[the_turnpoint].Bisector) < 180) bisectorOverpassed = AngleInRange(Reciprocal(Task[the_turnpoint].Bisector),Task[the_turnpoint].Bisector,Calculated->WaypointBearing,true); else bisectorOverpassed = AngleInRange(Task[the_turnpoint].Bisector,Reciprocal(Task[the_turnpoint].Bisector),Calculated->WaypointBearing,true); UnlockTaskData(); if(bisectorOverpassed) return true; } break; default: //Unknown sector type break; } return false; }
Vec3 ComputePrincipleComponent( Sym3x3 const& matrix ) { Vec4 const row0( matrix[0], matrix[1], matrix[2], 0.0f ); Vec4 const row1( matrix[1], matrix[3], matrix[4], 0.0f ); Vec4 const row2( matrix[2], matrix[4], matrix[5], 0.0f ); //Vec4 v = VEC4_CONST( 1.0f ); //Vec4 v = row0; // row1, row2 Vec3 v3 = EstimatePrincipleComponent( matrix ); Vec4 v( v3.X(), v3.Y(), v3.Z(), 0.0f ); for( int i = 0; i < POWER_ITERATION_COUNT; ++i ) { // matrix multiply Vec4 w = row0*v.SplatX(); w = MultiplyAdd(row1, v.SplatY(), w); w = MultiplyAdd(row2, v.SplatZ(), w); // get max component from xyz in all channels Vec4 a = Max(w.SplatX(), Max(w.SplatY(), w.SplatZ())); // divide through and advance v = w*Reciprocal(a); } return v.GetVec3(); }
void ColourNormalFit::Permute3() { const Vec3 scale = Vec3( 1.0f / 0.5f); const Vec3 offset = Vec3(-1.0f * 0.5f); const Vec3 scalei = Vec3( 1.0f * 0.5f); // cache some values int const count = m_colours->GetCount(); Vec3 const* values = m_colours->GetPoints(); Scr3 const* freq = m_colours->GetWeights(); cQuantizer3<5,6,5> q = cQuantizer3<5,6,5>(); Scr3 berror = Scr3(DEVIANCE_MAXSUM); Vec3 c_start = m_start; Vec3 c_end = m_end; Scr3 l_start = LengthSquared(Normalize(scale * (offset + m_start))); Scr3 l_end = LengthSquared(Normalize(scale * (offset + m_end))); Vec3 q_start = Reciprocal(q.grid + Vec3(1.0f)); Vec3 q_end = q_start; // adjust offset towards sphere-boundary if (!(l_start < Scr3(1.0f))) q_start = Vec3(0.0f) - q_start; if (!(l_end < Scr3(1.0f))) q_end = Vec3(0.0f) - q_end; int trie = 0x3F; do { // permute end-points +-1 towards sphere-boundary Vec3 p_start = q_start & Vec3(!(trie & 0x01), !(trie & 0x02), !(trie & 0x04)); Vec3 p_end = q_end & Vec3(!(trie & 0x08), !(trie & 0x10), !(trie & 0x20)); p_start = q.SnapToLattice(c_start + p_start); p_end = q.SnapToLattice(c_end + p_end); // create a codebook // resolve "metric * (value - code)" to "metric * value - metric * code" Vec3 codes[3]; Codebook3n(codes, p_start, p_end); Scr3 merror = Scr3(DEVIANCE_BASE); for (int i = 0; i < count; ++i) { // find the closest code Vec3 value = Normalize(scale * (offset + values[i])); Scr3 dist; MinDeviance3<false>(dist, i, value, codes); // accumulate the error AddDeviance(dist, merror, freq[i]); } if (berror > merror) { berror = merror; m_start = p_start; m_end = p_end; } } while(--trie); }
void RenderParams::update(ConstCDLOpDataRcPtr & cdl) { double slope[4], offset[4], power[4]; cdl->getSlopeParams().getRGBA(slope); cdl->getOffsetParams().getRGBA(offset); cdl->getPowerParams().getRGBA(power); const float saturation = (float)cdl->getSaturation(); const CDLOpData::Style style = cdl->getStyle(); m_isReverse = (style == CDLOpData::CDL_V1_2_REV) || (style == CDLOpData::CDL_NO_CLAMP_REV); m_isNoClamp = (style == CDLOpData::CDL_NO_CLAMP_FWD) || (style == CDLOpData::CDL_NO_CLAMP_REV); if (isReverse()) { // Reverse render parameters setSlope(Reciprocal((float)slope[0]), Reciprocal((float)slope[1]), Reciprocal((float)slope[2]), Reciprocal((float)slope[3])); setOffset((float)-offset[0], (float)-offset[1], (float)-offset[2], (float)-offset[3]); setPower(Reciprocal((float)power[0]), Reciprocal((float)power[1]), Reciprocal((float)power[2]), Reciprocal((float)power[3])); setSaturation(Reciprocal(saturation)); } else { // Forward render parameters setSlope((float)slope[0], (float)slope[1], (float)slope[2], (float)slope[3]); setOffset((float)offset[0], (float)offset[1], (float)offset[2], (float)offset[3]); setPower((float)power[0], (float)power[1], (float)power[2], (float)power[3]); setSaturation(saturation); } }
int main (void) { int a[11]; //maximum coeficient// int i, N; do { //A do-while loop for the programme to repeat itself if it isnt self-reciprocal// printf("\nPlease enter the maximum degree of the polynomial\n"); //Asks user for input// scanf("%d", &N); printf("Please enter the coefficients\n"); //Read the coefficients into an array// for (i=0;i <= N;i++) //A For loop to that will ask the user for 1 more number than the maximum degree // { scanf("%d",&a[i]); } //this terminates the programme when the user enters a order of less than 0// if(N<0) { return 0; } Poly(a,N); //prints out the polynomial function// printf("\n"); Reciprocal(a,N); //prints out the recirocal polynomial function// printf("\n"); Testself(a,N); //determines and prints wether the polynomial is self or none self-reciprocal function// if(Testself(a,N)==1) printf("\nThe polynomial is self-reciprocal\n"); //this is the if statement to test wether the polynomial is self or non self-reciprocal// else printf("\nThe polynomial is not self-reciprocal\n"); } while(Testself(a,N)!=1); //part of the do-while loop to repeat programme when it itsnt self-reciprocal, and terminates the programme when it is sel-reciprocal// getch(); }
Angle Angle::HalfAngle(const Angle end) const { if (value == end.value) { return Reciprocal(); } else if (value > end.value) { if ((*this - end) < HalfCircle()) return (*this + end).Half().Reciprocal(); else return (*this + end).Half(); } else { if ((end - *this) < HalfCircle()) return (*this + end).Half().Reciprocal(); else return (*this + end).Half(); } }
Angle Angle::HalfAngle(const Angle &End) const { if (m_value == End.m_value) { return Reciprocal(); } else if (m_value > End.m_value) { if ((m_value - End.m_value) < fixed_half_circle) return (*this + End).half().Reciprocal(); else return (*this + End).half(); } else { if ((End.m_value - m_value) < fixed_half_circle) return (*this + End).half().Reciprocal(); else return (*this + End).half(); } }
void SingleThread(){ std::vector<unsigned char> Recs; std::ostringstream Line; unsigned int Num = GetNextPrime(); std::cout << Num << std::endl; while (Num != 0) { Recs = Reciprocal(Num); Line << Num << " 0."; for (int i = 1; i < Recs.size(); i++) { Line << (int)Recs[i]; } if(Recs.size() == Num) { AddToSONum(Line.str()); }else if(Recs.size() < Num) { AddToLTNum(Line.str()); } else { AddToGTNum(Line.str()); } Line.str(""); Num = GetNextPrime(); } }
Angle Angle::BiSector(const Angle &OutBound) const { return Reciprocal().HalfAngle(OutBound); }
Vec4 ClusterFit::SolveLeastSquares( Vec4& start, Vec4& end ) const { // accumulate all the quantities we need int const count = m_colours->GetCount(); Vec4 alpha2_sum = VEC4_CONST( 0.0f ); Vec4 beta2_sum = VEC4_CONST( 0.0f ); Vec4 alphabeta_sum = VEC4_CONST( 0.0f ); Vec4 alphax_sum = VEC4_CONST( 0.0f ); Vec4 betax_sum = VEC4_CONST( 0.0f ); for( int i = 0; i < count; ++i ) { Vec4 alpha = m_alpha[i]; Vec4 beta = m_beta[i]; Vec4 x = m_weighted[i]; alpha2_sum = MultiplyAdd( alpha, alpha, alpha2_sum ); beta2_sum = MultiplyAdd( beta, beta, beta2_sum ); alphabeta_sum = MultiplyAdd( alpha, beta, alphabeta_sum ); alphax_sum = MultiplyAdd( alpha, x, alphax_sum ); betax_sum = MultiplyAdd( beta, x, betax_sum ); } // select the results Vec4 const zero = VEC4_CONST( 0.0f ); Vec4 beta2_sum_zero = CompareEqual( beta2_sum, zero ); Vec4 alpha2_sum_zero = CompareEqual( alpha2_sum, zero ); Vec4 a1 = alphax_sum*Reciprocal( alpha2_sum ); Vec4 b1 = betax_sum*Reciprocal( beta2_sum ); Vec4 factor = Reciprocal( NegativeMultiplySubtract( alphabeta_sum, alphabeta_sum, alpha2_sum*beta2_sum ) ); Vec4 a2 = NegativeMultiplySubtract( betax_sum, alphabeta_sum, alphax_sum*beta2_sum )*factor; Vec4 b2 = NegativeMultiplySubtract( alphax_sum, alphabeta_sum, betax_sum*alpha2_sum )*factor; Vec4 a = Select( Select( a2, a1, beta2_sum_zero ), zero, alpha2_sum_zero ); Vec4 b = Select( Select( b2, b1, alpha2_sum_zero ), zero, beta2_sum_zero ); // clamp the output to [0, 1] Vec4 const one = VEC4_CONST( 1.0f ); Vec4 const half = VEC4_CONST( 0.5f ); a = Min( one, Max( zero, a ) ); b = Min( one, Max( zero, b ) ); // clamp to the grid Vec4 const grid( 31.0f, 63.0f, 31.0f, 0.0f ); Vec4 const gridrcp( 1.0f/31.0f, 1.0f/63.0f, 1.0f/31.0f, 0.0f ); Vec4 const onethird = VEC4_CONST( 1.0f/3.0f ); Vec4 const twothirds = VEC4_CONST( 2.0f/3.0f ); a = Truncate( MultiplyAdd( grid, a, half ) )*gridrcp; b = Truncate( MultiplyAdd( grid, b, half ) )*gridrcp; // compute the error Vec4 const two = VEC4_CONST( 2.0 ); Vec4 e1 = MultiplyAdd( b*b, beta2_sum, m_xxsum ); Vec4 e2 = MultiplyAdd( a, alphax_sum, b*betax_sum ); Vec4 e3 = MultiplyAdd( a*a, alpha2_sum, e1 ); Vec4 e4 = MultiplyAdd( a*b*alphabeta_sum - e2, two, e3 ); // apply the metric to the error term Vec4 e5 = e4*m_metricSqr; Vec4 error = e5.SplatX() + e5.SplatY() + e5.SplatZ(); // save the start and end start = a; end = b; return error; }
ColourNormalFit::ColourNormalFit(ColourSet const* colours, int flags) : ColourFit(colours, flags) { cQuantizer3<5,6,5> q = cQuantizer3<5,6,5>(); // cache some values int const count = m_colours->GetCount(); Vec3 const* values = m_colours->GetPoints(); Scr3 const* weights = m_colours->GetWeights(); #ifdef FEATURE_NORMALFIT_PROJECT Sym3x3 covariance; Vec3 centroid; Vec3 principle; // get the covariance matrix if (m_colours->IsUnweighted()) ComputeWeightedCovariance3(covariance, centroid, count, values, Vec3(1.0f)); else ComputeWeightedCovariance3(covariance, centroid, count, values, Vec3(1.0f), weights); // compute the principle component GetPrincipleComponent(covariance, principle); // get the min and max normal as the codebook endpoints Vec3 start(127.5f, 127.5f, 255.0f); Vec3 end(127.5f, 127.5f, 255.0f); if (count > 0) { #undef FEATURE_NORMALFIT_PROJECT_NEAREST #ifdef FEATURE_NORMALFIT_PROJECT_NEAREST const Vec3 scale = Vec3( 1.0f / 0.5f); const Vec3 offset = Vec3(-1.0f * 0.5f); const Vec3 scalei = Vec3( 1.0f * 0.5f); Vec3 centroidn = (scale * (offset + centroid)); Vec3 rec = Reciprocal(principle); Scr3 min, max; Vec3 chk; // http://geomalgorithms.com/a07-_distance.html // compute the line parameters of the two closest points min = Scr3( FLT_MAX); max = Scr3(-FLT_MAX); for (int i = 0; i < count; ++i) { Vec3 valuenorm = Normalize(scale * (offset + values[i])); Vec3 u = principle;//L1.P1 - L1.P0; Vec3 v = valuenorm;//L2.P1 - L2.P0; Vec3 w = centroidn;//L1.P0 - L2.P0; Scr3 a = Dot(u, u); // always >= 0 Scr3 b = Dot(u, v); Scr3 c = Dot(v, v); // always >= 0 Scr3 d = Dot(u, w); Scr3 e = Dot(v, w); Scr3 D = a * c - b * b; // always >= 0 Scr3 sc, tc; // compute the line parameters of the two closest points if (D < Scr3(0.00001f)) { // the lines are almost parallel sc = Scr3(0.0f); // use the largest denominator tc = (b > c ? d * Reciprocal(b) : e * Reciprocal(c) ); } else { D = Reciprocal(D); sc = (b * e - c * d) * D; tc = (a * e - b * d) * D; } // one dimension of the principle axis is 1 // the maximum magnitude the principle axis // can move in the [-1,+1] cube is 1.41*2 // without leaving the cube's boundaries sc = Min(sc, Scr3( 2.82842712474619f)); sc = Max(sc, Scr3(-2.82842712474619f)); min = Min(min, sc); max = Max(max, sc); } start = centroidn + principle * min; end = centroidn + principle * max; start = Normalize(start); end = Normalize(end ); start = (start * scalei) - offset; end = (end * scalei) - offset; #else // compute the projection GetPrincipleProjection(start, end, principle, centroid, count, values); #endif #else Scr3 min, max; // compute the normal start = end = values[0]; min = max = Dot(values[0], principle); for (int i = 1; i < count; ++i) { Scr3 val = Dot(values[i], principle); if (val < min) { start = values[i]; min = val; } else if (val > max) { end = values[i]; max = val; } } #endif } // snap floating-point-values to the integer-lattice and save m_start_candidate = q.SnapToLattice(start); m_end_candidate = q.SnapToLattice(end ); }
void BitoneClusterFit::ClusterFit4(void* block) { // declare variables int const count = m_bitones->GetCount(); Vec4 const two = VEC4_CONST(2.0f); Vec4 const one = VEC4_CONST(1.0f); Vec4 const onethird_onethird2 (1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 9.0f); Vec4 const twothirds_twothirds2(2.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f, 4.0f / 9.0f); Vec4 const twonineths = VEC4_CONST(2.0f / 9.0f); assume((count > 0) && (count <= 16)); // prepare an ordering using the principle axis ConstructOrdering(m_principle, 0); // check all possible clusters and iterate on the total order Vec4 beststart = VEC4_CONST(0.0f); Vec4 bestend = VEC4_CONST(0.0f); Scr4 besterror = m_besterror; u8 bestindices[16]; int bestiteration = 0; int besti = 0, bestj = 0, bestk = 0; // loop over iterations (we avoid the case that all points in first or last cluster) for (int iterationIndex = 0;;) { // first cluster [0,i) is at the start Vec4 part0 = VEC4_CONST(0.0f); for (int i = 0; i < count; ++i) { // second cluster [i,j) is one third along Vec4 part1 = VEC4_CONST(0.0f); for (int j = i;;) { // third cluster [j,k) is two thirds along Vec4 part2 = (j == 0) ? m_points_weights[0] : VEC4_CONST(0.0f); int kmin = (j == 0) ? 1 : j; for (int k = kmin;;) { // last cluster [k,count) is at the end Vec4 part3 = m_xsum_wsum - part2 - part1 - part0; // compute least squares terms directly Vec4 const alphax_sum = MultiplyAdd(part2, onethird_onethird2, MultiplyAdd(part1, twothirds_twothirds2, part0)); Vec4 const betax_sum = MultiplyAdd(part1, onethird_onethird2, MultiplyAdd(part2, twothirds_twothirds2, part3)); Vec4 const alpha2_sum = alphax_sum.SplatW(); Vec4 const beta2_sum = betax_sum.SplatW(); Vec4 const alphabeta_sum = twonineths * (part1 + part2).SplatW(); // compute the least-squares optimal points Vec4 factor = Reciprocal(NegativeMultiplySubtract(alphabeta_sum, alphabeta_sum, alpha2_sum * beta2_sum)); Vec4 a = NegativeMultiplySubtract( betax_sum, alphabeta_sum, alphax_sum * beta2_sum) * factor; Vec4 b = NegativeMultiplySubtract(alphax_sum, alphabeta_sum, betax_sum * alpha2_sum) * factor; // snap floating-point-values to the integer-lattice a = Truncate(a * 255.0f) * (1.0f / 255.0f); b = Truncate(b * 255.0f) * (1.0f / 255.0f); // compute the error (we skip the constant xxsum) Vec4 e1 = MultiplyAdd(a * a, alpha2_sum, b * b * beta2_sum); Vec4 e2 = NegativeMultiplySubtract(a, alphax_sum, a * b * alphabeta_sum); Vec4 e3 = NegativeMultiplySubtract(b, betax_sum, e2); Vec4 e4 = MultiplyAdd(two, e3, e1); // apply the metric to the error term Scr4 eS = e4; // keep the solution if it wins if (besterror > eS) { besterror = eS; beststart = a; bestend = b; besti = i; bestj = j; bestk = k; bestiteration = iterationIndex; } // advance if (k == count) break; part2 += m_points_weights[k]; ++k; } // advance if (j == count) break; part1 += m_points_weights[j]; ++j; } // advance part0 += m_points_weights[i]; } // stop if we didn't improve in this iteration if (bestiteration != iterationIndex) break; // advance if possible ++iterationIndex; if (iterationIndex == m_iterationCount) break; // stop if a new iteration is an ordering that has already been tried Vec3 axis = (bestend - beststart).GetVec3(); if (!ConstructOrdering(axis, iterationIndex)) break; } // save the block if necessary if (besterror < m_besterror) { // save the error m_besterror = besterror; // remap the indices u8 const* order = (u8*)m_order + 16 * bestiteration; u8 unordered[16]; for (int m = 0; m < besti; ++m) unordered[order[m]] = 0; for (int m = besti; m < bestj; ++m) unordered[order[m]] = 2; for (int m = bestj; m < bestk; ++m) unordered[order[m]] = 3; for (int m = bestk; m < count; ++m) unordered[order[m]] = 1; m_bitones->RemapIndices(unordered, bestindices); // save the block WriteBitoneBlock4(beststart.GetVec3(), bestend.GetVec3(), bestindices, block); } }
void MapWindow::DrawTask(HDC hdc, RECT rc, const POINT &Orig_Aircraft) { int i; double tmp; COLORREF whitecolor = RGB_WHITE; COLORREF origcolor = SetTextColor(hDCTemp, whitecolor); HPEN oldpen = 0; HBRUSH oldbrush = 0; static short size_tasklines=0; if (DoInit[MDI_DRAWTASK]) { switch (ScreenSize) { case ss480x272: case ss272x480: case ss320x240: case ss240x320: size_tasklines=NIBLSCALE(4); break; default: size_tasklines=NIBLSCALE(3); break; } DoInit[MDI_DRAWTASK]=false; } if (!WayPointList) return; oldpen = (HPEN) SelectObject(hdc, hpStartFinishThick); oldbrush = (HBRUSH) SelectObject(hdc, GetStockObject(HOLLOW_BRUSH)); LockTaskData(); // protect from external task changes for (i = 1; ValidTaskPoint(i); i++) { if (!ValidTaskPoint(i + 1)) { // final waypoint if (ActiveWayPoint > 1 || !ValidTaskPoint(2)) { // only draw finish line when past the first // waypoint. FIXED 110307: or if task is with only 2 tps DrawStartEndSector(hdc, rc, Task[i].Start, Task[i].End, Task[i].Index, FinishLine, FinishRadius); } } else { // normal sector if (AATEnabled != TRUE) { //_DrawLine(hdc, PS_DASH, NIBLSCALE(3), WayPointList[Task[i].Index].Screen, Task[i].Start, RGB_PETROL, rc); //_DrawLine(hdc, PS_DASH, NIBLSCALE(3), WayPointList[Task[i].Index].Screen, Task[i].End, RGB_PETROL, rc); // DrawDashLine(hdc, size_tasklines, WayPointList[Task[i].Index].Screen, Task[i].Start, RGB_PETROL, rc); // DrawDashLine(hdc, size_tasklines, WayPointList[Task[i].Index].Screen, Task[i].End, RGB_PETROL, rc); } int Type = 0; double Radius = 0.; GetTaskSectorParameter(i, &Type, &Radius); switch (Type) { case CIRCLE: tmp = Radius * zoom.ResScaleOverDistanceModify(); Circle(hdc, WayPointList[Task[i].Index].Screen.x, WayPointList[Task[i].Index].Screen.y, (int) tmp, rc, false, false); break; case SECTOR: tmp = Radius * zoom.ResScaleOverDistanceModify(); Segment(hdc, WayPointList[Task[i].Index].Screen.x, WayPointList[Task[i].Index].Screen.y, (int) tmp, rc, Task[i].AATStartRadial - DisplayAngle, Task[i].AATFinishRadial - DisplayAngle); break; case DAe: if (!AATEnabled) { // this Type exist only if not AAT task // JMW added german rules tmp = 500 * zoom.ResScaleOverDistanceModify(); Circle(hdc, WayPointList[Task[i].Index].Screen.x, WayPointList[Task[i].Index].Screen.y, (int) tmp, rc, false, false); tmp = 10e3 * zoom.ResScaleOverDistanceModify(); Segment(hdc, WayPointList[Task[i].Index].Screen.x, WayPointList[Task[i].Index].Screen.y, (int) tmp, rc, Task[i].AATStartRadial - DisplayAngle, Task[i].AATFinishRadial - DisplayAngle); } break; case LINE: if (!AATEnabled) { // this Type exist only if not AAT task if(ISGAAIRCRAFT) { POINT start,end; double rotation=AngleLimit360(Task[i].Bisector-DisplayAngle); int length=14*ScreenScale; //Make intermediate WP lines always of the same size independent by zoom level start.x=WayPointList[Task[i].Index].Screen.x+(long)(length*fastsine(rotation)); start.y=WayPointList[Task[i].Index].Screen.y-(long)(length*fastcosine(rotation)); rotation=Reciprocal(rotation); end.x=WayPointList[Task[i].Index].Screen.x+(long)(length*fastsine(rotation)); end.y=WayPointList[Task[i].Index].Screen.y-(long)(length*fastcosine(rotation)); _DrawLine(hdc, PS_SOLID, NIBLSCALE(3), start, end, taskcolor, rc); } else _DrawLine(hdc, PS_SOLID, NIBLSCALE(3), Task[i].Start, Task[i].End, taskcolor, rc); } break; case CONE: tmp = Radius * zoom.ResScaleOverDistanceModify(); int center_x = WayPointList[Task[i].Index].Screen.x; int center_y = WayPointList[Task[i].Index].Screen.y; Circle(hdc, center_x, center_y, (int) tmp, rc, false, false); HPEN prevPen = (HPEN)::SelectObject(hdc, hpTerrainLine); for( int j = 1; j < 5 && tmp > 0; ++j) { Circle(hdc, center_x, center_y, tmp -= NIBLSCALE(5), rc, true, true); } ::SelectObject(hdc, prevPen); break; } if (AATEnabled && !DoOptimizeRoute()) { // ELSE HERE IS *** AAT *** // JMW added iso lines if ((i == ActiveWayPoint) || (mode.Is(Mode::MODE_TARGET_PAN) && (i == TargetPanIndex))) { // JMW 20080616 flash arc line if very close to target static bool flip = false; if (DerivedDrawInfo.WaypointDistance < AATCloseDistance()*2.0) { flip = !flip; } else { flip = true; } if (flip) { for (int j = 0; j < MAXISOLINES - 1; j++) { if (TaskStats[i].IsoLine_valid[j] && TaskStats[i].IsoLine_valid[j + 1]) { _DrawLine(hdc, PS_SOLID, NIBLSCALE(2), TaskStats[i].IsoLine_Screen[j], TaskStats[i].IsoLine_Screen[j + 1], RGB(0, 0, 255), rc); } } } } } } } if ((ActiveWayPoint < 2) && ValidTaskPoint(0) && ValidTaskPoint(1)) { DrawStartEndSector(hdc, rc, Task[0].Start, Task[0].End, Task[0].Index, StartLine, StartRadius); if (EnableMultipleStartPoints) { for (i = 0; i < MAXSTARTPOINTS; i++) { if (StartPoints[i].Active && ValidWayPoint(StartPoints[i].Index)) { DrawStartEndSector(hdc, rc, StartPoints[i].Start, StartPoints[i].End, StartPoints[i].Index, StartLine, StartRadius); } } } } for (i = 0; ValidTaskPoint(i + 1); i++) { int imin = min(Task[i].Index, Task[i + 1].Index); int imax = max(Task[i].Index, Task[i + 1].Index); // JMW AAT! double bearing = Task[i].OutBound; POINT sct1, sct2; if (AATEnabled) { LatLon2Screen(Task[i].AATTargetLon, Task[i].AATTargetLat, sct1); LatLon2Screen(Task[i + 1].AATTargetLon, Task[i + 1].AATTargetLat, sct2); DistanceBearing(Task[i].AATTargetLat, Task[i].AATTargetLon, Task[i + 1].AATTargetLat, Task[i + 1].AATTargetLon, NULL, &bearing); // draw nominal track line DrawDashLine(hdc, NIBLSCALE(1), // 091217 WayPointList[imin].Screen, WayPointList[imax].Screen, taskcolor, rc); } else { sct1 = WayPointList[imin].Screen; sct2 = WayPointList[imax].Screen; } if ((i >= ActiveWayPoint && DoOptimizeRoute()) || !DoOptimizeRoute()) { POINT ClipPt1 = sct1, ClipPt2 = sct2; if(LKGeom::ClipLine((POINT) {rc.left, rc.top}, (POINT) {rc.right, rc.bottom}, ClipPt1, ClipPt2)) { DrawMulticolorDashLine(hdc, size_tasklines, sct1, sct2, taskcolor, RGB_BLACK,rc); // draw small arrow along task direction POINT p_p; POINT Arrow[2] = { {6, 6}, {-6, 6} }; ScreenClosestPoint(sct1, sct2, Orig_Aircraft, &p_p, NIBLSCALE(25)); threadsafePolygonRotateShift(Arrow, 2, p_p.x, p_p.y, bearing - DisplayAngle); _DrawLine(hdc, PS_SOLID, size_tasklines-NIBLSCALE(1), Arrow[0], p_p, taskcolor, rc); _DrawLine(hdc, PS_SOLID, size_tasklines-NIBLSCALE(1), Arrow[1], p_p, taskcolor, rc); } } } // Draw DashLine From current position to Active TurnPoint center if(ValidTaskPoint(ActiveWayPoint)) { POINT ptStart; LatLon2Screen(DrawInfo.Longitude, DrawInfo.Latitude, ptStart); DrawDashLine(hdc, NIBLSCALE(1), ptStart, WayPointList[Task[ActiveWayPoint].Index].Screen, taskcolor, rc); } { UnlockTaskData(); } // restore original color SetTextColor(hDCTemp, origcolor); SelectObject(hdc, oldpen); SelectObject(hdc, oldbrush); }
void WeightedClusterFit::Compress4( void* block ) { int const count = m_colours->GetCount(); Vec4 const one = VEC4_CONST(1.0f); Vec4 const zero = VEC4_CONST(0.0f); Vec4 const half = VEC4_CONST(0.5f); Vec4 const two = VEC4_CONST(2.0); Vec4 const onethird( 1.0f/3.0f, 1.0f/3.0f, 1.0f/3.0f, 1.0f/9.0f ); Vec4 const twothirds( 2.0f/3.0f, 2.0f/3.0f, 2.0f/3.0f, 4.0f/9.0f ); Vec4 const twonineths = VEC4_CONST( 2.0f/9.0f ); Vec4 const grid( 31.0f, 63.0f, 31.0f, 0.0f ); Vec4 const gridrcp( 1.0f/31.0f, 1.0f/63.0f, 1.0f/31.0f, 0.0f ); // declare variables Vec4 beststart = VEC4_CONST( 0.0f ); Vec4 bestend = VEC4_CONST( 0.0f ); Vec4 besterror = VEC4_CONST( FLT_MAX ); Vec4 x0 = zero; int b0 = 0, b1 = 0, b2 = 0; // check all possible clusters for this total order for( int c0 = 0; c0 < count; c0++) { Vec4 x1 = zero; for( int c1 = 0; c1 < count-c0; c1++) { Vec4 x2 = zero; for( int c2 = 0; c2 < count-c0-c1; c2++) { Vec4 const x3 = m_xsum - x2 - x1 - x0; //Vec3 const alphax_sum = x0 + x1 * (2.0f / 3.0f) + x2 * (1.0f / 3.0f); //float const alpha2_sum = w0 + w1 * (4.0f/9.0f) + w2 * (1.0f/9.0f); Vec4 const alphax_sum = MultiplyAdd(x2, onethird, MultiplyAdd(x1, twothirds, x0)); // alphax_sum, alpha2_sum Vec4 const alpha2_sum = alphax_sum.SplatW(); //Vec3 const betax_sum = x3 + x2 * (2.0f / 3.0f) + x1 * (1.0f / 3.0f); //float const beta2_sum = w3 + w2 * (4.0f/9.0f) + w1 * (1.0f/9.0f); Vec4 const betax_sum = MultiplyAdd(x2, twothirds, MultiplyAdd(x1, onethird, x3)); // betax_sum, beta2_sum Vec4 const beta2_sum = betax_sum.SplatW(); //float const alphabeta_sum = (w1 + w2) * (2.0f/9.0f); Vec4 const alphabeta_sum = twonineths*( x1 + x2 ).SplatW(); // alphabeta_sum // float const factor = 1.0f / (alpha2_sum * beta2_sum - alphabeta_sum * alphabeta_sum); Vec4 const factor = Reciprocal( NegativeMultiplySubtract(alphabeta_sum, alphabeta_sum, alpha2_sum*beta2_sum) ); Vec4 a = NegativeMultiplySubtract(betax_sum, alphabeta_sum, alphax_sum*beta2_sum) * factor; Vec4 b = NegativeMultiplySubtract(alphax_sum, alphabeta_sum, betax_sum*alpha2_sum) * factor; // clamp to the grid a = Min( one, Max( zero, a ) ); b = Min( one, Max( zero, b ) ); a = Truncate( MultiplyAdd( grid, a, half ) ) * gridrcp; b = Truncate( MultiplyAdd( grid, b, half ) ) * gridrcp; // compute the error (we skip the constant xxsum) Vec4 e1 = MultiplyAdd( a*a, alpha2_sum, b*b*beta2_sum ); Vec4 e2 = NegativeMultiplySubtract( a, alphax_sum, a*b*alphabeta_sum ); Vec4 e3 = NegativeMultiplySubtract( b, betax_sum, e2 ); Vec4 e4 = MultiplyAdd( two, e3, e1 ); // apply the metric to the error term Vec4 e5 = e4 * m_metricSqr; Vec4 error = e5.SplatX() + e5.SplatY() + e5.SplatZ(); // keep the solution if it wins if( CompareAnyLessThan( error, besterror ) ) { besterror = error; beststart = a; bestend = b; b0 = c0; b1 = c1; b2 = c2; } x2 += m_weighted[c0+c1+c2]; } x1 += m_weighted[c0+c1]; } x0 += m_weighted[c0]; } // save the block if necessary if( CompareAnyLessThan( besterror, m_besterror ) ) { // compute indices from cluster sizes. u8 bestindices[16]; { int i = 0; for(; i < b0; i++) { bestindices[i] = 0; } for(; i < b0+b1; i++) { bestindices[i] = 2; } for(; i < b0+b1+b2; i++) { bestindices[i] = 3; } for(; i < count; i++) { bestindices[i] = 1; } } // remap the indices u8 ordered[16]; for( int i = 0; i < count; ++i ) ordered[m_order[i]] = bestindices[i]; m_colours->RemapIndices( ordered, bestindices ); // save the block WriteColourBlock4( beststart.GetVec3(), bestend.GetVec3(), bestindices, block ); // save the error m_besterror = besterror; } }
Angle Angle::BiSector(const Angle out_bound) const { return Reciprocal().HalfAngle(out_bound); }
void BitoneClusterFit::ClusterFit4Constant(void* block) { // declare variables int const count = m_bitones->GetCount(); Vec4 const two = VEC4_CONST(2.0f); Vec4 const one = VEC4_CONST(1.0f); Vec4 const onethird_onethird2 (1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 3.0f, 1.0f / 9.0f); Vec4 const twothirds_twothirds2(2.0f / 3.0f, 2.0f / 3.0f, 2.0f / 3.0f, 4.0f / 9.0f); Vec4 const twonineths = VEC4_CONST(2.0f / 9.0f); assume((count > 0) && (count <= 16)); // check all possible clusters and iterate on the total order Vec4 beststart = VEC4_CONST(0.0f); Vec4 bestend = VEC4_CONST(0.0f); Scr4 besterror = m_besterror; u8 bestindices[16]; int bestiteration = 0; int besti = 0, bestj = 0, bestk = 0; // prepare an ordering using the principle axis ConstructOrdering(m_principle, 0); // loop over iterations (we avoid the case that all points in first or last cluster) for (int iterationIndex = 0;;) { // cache some values Vec4 const xsum_wsum = m_xsum_wsum; // constants if weights == 1 Vec4 alphabeta_dltas = *((Vec4 *)part2delta[0]); Vec4 *alphabeta_inits = (Vec4 *)part2inits[0]; float *alphabeta_factors = (float *)part2factors; #if 0 Vec4 lasta = Vec4(0.0f); Vec4 lastb = xsum_wsum; Vec4 lastc = Vec4(0.0f); #endif // first cluster [0,i) is at the start Vec4 part0 = VEC4_CONST(0.0f); for (int i = 0; i < count; ++i) { // second cluster [i,j) is one third along Vec4 part1 = VEC4_CONST(0.0f); for (int j = i;;) { // third cluster [j,k) is two thirds along Vec4 part2 = (j == 0) ? m_points_weights[0] : VEC4_CONST(0.0f); Vec4 alphabeta_val = *alphabeta_inits++; int kmin = (j == 0) ? 1 : j; for (int k = kmin;;) { // TODO: the inner alphabeta_sum seems always to be the same sequence Vec4 alphabeta_factor = alphabeta_val * Vec4(*alphabeta_factors++); // compute least squares terms directly Vec4 const alphax_sum = MultiplyAdd(part2, onethird_onethird2, MultiplyAdd(part1, twothirds_twothirds2, part0)); Vec4 const betax_sum = /*MultiplyAdd(part1, onethird_onethird2, MultiplyAdd(part2, twothirds_twothirds2, part3))*/ xsum_wsum - alphax_sum; Vec4 const alpha2_sum = alphabeta_val.SplatX(); Vec4 const beta2_sum = alphabeta_val.SplatY(); Vec4 const alphabeta_sum = alphabeta_val.SplatZ(); Vec4 a = NegativeMultiplySubtract( betax_sum, alphabeta_factor.SplatZ(), alphax_sum * alphabeta_factor.SplatY()); Vec4 b = NegativeMultiplySubtract(alphax_sum, alphabeta_factor.SplatZ(), betax_sum * alphabeta_factor.SplatX()); #if 0 // last cluster [k,count) is at the end Vec4 part3 = xsum_wsum - part2 - part1 - part0; // compute least squares terms directly Vec4 const _alphax_sum = MultiplyAdd(part2, onethird_onethird2, MultiplyAdd(part1, twothirds_twothirds2, part0)); Vec4 const _betax_sum = MultiplyAdd(part1, onethird_onethird2, MultiplyAdd(part2, twothirds_twothirds2, part3)); // Vec4 const _betac_sum = xsum_wsum - _alphax_sum; Vec4 const _alpha2_sum = _alphax_sum.SplatW(); Vec4 const _beta2_sum = _betax_sum.SplatW(); Vec4 const _alphabeta_sum = twonineths * (part1 + part2).SplatW(); // compute the least-squares optimal points Vec4 _factor = Reciprocal(NegativeMultiplySubtract(_alphabeta_sum, _alphabeta_sum, _alpha2_sum * _beta2_sum)); Vec4 _a = NegativeMultiplySubtract( _betax_sum, _alphabeta_sum, _alphax_sum * _beta2_sum) * _factor; Vec4 _b = NegativeMultiplySubtract(_alphax_sum, _alphabeta_sum, _betax_sum * _alpha2_sum) * _factor; #undef limit #define limit 2e-5 assert(fabs(_alpha2_sum.W() - alpha2_sum.X()) < limit); assert(fabs(_beta2_sum.W() - beta2_sum.X()) < limit); assert(fabs(_alphabeta_sum.W() - alphabeta_sum.X()) < limit); if (alphabeta_factors[-1] != FLT_MAX) { assert(fabs(_factor.W() - alphabeta_factors[-1]) < limit); assert(fabs(_alpha2_sum.W() * _factor.W() - alphabeta_factor.X()) < limit); assert(fabs(_beta2_sum.W() * _factor.W() - alphabeta_factor.Y()) < limit); assert(fabs(_alphabeta_sum.W() * _factor.W() - alphabeta_factor.Z()) < limit); assert(fabs(a.X() - _a.X()) < limit); assert(fabs(a.Y() - _a.Y()) < limit); assert(fabs(a.Z() - _a.Z()) < limit); assert(fabs(b.X() - _b.X()) < limit); assert(fabs(b.Y() - _b.Y()) < limit); assert(fabs(b.Z() - _b.Z()) < limit); } #if 0 fprintf(stderr, "{%.9ff},", _factor.W()); if (k == kmin) fprintf(stderr, "{%.9f, %.9f, %.9f},\n", alpha2_sum.W(), beta2_sum.W(), alphabeta_sum.W()); fprintf(stderr, "{%.9f/*%.9f*/,%.9f/*%.9f*/,%.9f,%.9f},\n", alpha2_sum.W(), lasta.W() - alpha2_sum.W(), beta2_sum.W(), lastb.W() - beta2_sum.W(), alphabeta_sum.W(), lastc.W() - alphabeta_sum.W(), factor.W()); lasta = alpha2_sum; lastb = beta2_sum; lastc = alphabeta_sum; #endif #endif // snap floating-point-values to the integer-lattice a = Truncate(a * 255.0f) * (1.0f / 255.0f); b = Truncate(b * 255.0f) * (1.0f / 255.0f); // compute the error (we skip the constant xxsum) Vec4 e1 = MultiplyAdd(a * a, alpha2_sum, b * b * beta2_sum); Vec4 e2 = NegativeMultiplySubtract(a, alphax_sum, a * b * alphabeta_sum); Vec4 e3 = NegativeMultiplySubtract(b, betax_sum, e2); Vec4 e4 = MultiplyAdd(two, e3, e1); // apply the metric to the error term Scr4 eS = e4; // keep the solution if it wins if (besterror > eS) { besterror = eS; beststart = a; bestend = b; bestiteration = iterationIndex; besti = i; bestj = j; bestk = k; } alphabeta_val += alphabeta_dltas; // advance if (k == count) break; part2 += m_points_weights[k]; ++k; } // advance if (j == count) break; part1 += m_points_weights[j]; ++j; } // advance part0 += m_points_weights[i]; } // stop if we didn't improve in this iteration if (bestiteration != iterationIndex) break; // advance if possible ++iterationIndex; if (iterationIndex == m_iterationCount) break; // stop if a new iteration is an ordering that has already been tried Vec3 axis = (bestend - beststart).GetVec3(); if (!ConstructOrdering(axis, iterationIndex)) break; } // save the block if necessary if (besterror < m_besterror) { // save the error m_besterror = besterror; // remap the indices u8 const* order = (u8*)m_order + 16 * bestiteration; u8 unordered[16]; for (int m = 0; m < besti; ++m) unordered[order[m]] = 0; for (int m = besti; m < bestj; ++m) unordered[order[m]] = 2; for (int m = bestj; m < bestk; ++m) unordered[order[m]] = 3; for (int m = bestk; m < count; ++m) unordered[order[m]] = 1; m_bitones->RemapIndices(unordered, bestindices); // save the block WriteBitoneBlock4(beststart.GetVec3(), bestend.GetVec3(), bestindices, block); } }
void CalculateAATTaskSectors() { int i; int awp = ActiveTaskPoint; if(AATEnabled == FALSE || DoOptimizeRoute()) return; double latitude = GPS_INFO.Latitude; double longitude = GPS_INFO.Longitude; double altitude = GPS_INFO.Altitude; LockTaskData(); Task[0].AATTargetOffsetRadius = 0.0; Task[0].AATTargetOffsetRadial = 0.0; if (Task[0].Index>=0) { Task[0].AATTargetLat = WayPointList[Task[0].Index].Latitude; Task[0].AATTargetLon = WayPointList[Task[0].Index].Longitude; } for(i=1;i<MAXTASKPOINTS;i++) { if(ValidTaskPoint(i)) { if (!ValidTaskPoint(i+1)) { // This must be the final waypoint, so it's not an AAT OZ Task[i].AATTargetLat = WayPointList[Task[i].Index].Latitude; Task[i].AATTargetLon = WayPointList[Task[i].Index].Longitude; continue; } if(Task[i].AATType == SECTOR) { FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude, WayPointList[Task[i].Index].Longitude, Task[i].AATStartRadial , Task[i].AATSectorRadius , &Task[i].AATStartLat, &Task[i].AATStartLon); FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude, WayPointList[Task[i].Index].Longitude, Task[i].AATFinishRadial , Task[i].AATSectorRadius, &Task[i].AATFinishLat, &Task[i].AATFinishLon); } // JMWAAT: if locked, don't move it if (i<awp) { // only update targets for current/later waypoints continue; } Task[i].AATTargetOffsetRadius = min(1.0, max(Task[i].AATTargetOffsetRadius,-1.0)); Task[i].AATTargetOffsetRadial = min(90.0, max(-90.0, Task[i].AATTargetOffsetRadial)); double targetbearing; double targetrange; targetbearing = AngleLimit360(Task[i].Bisector+Task[i].AATTargetOffsetRadial); if(Task[i].AATType == SECTOR) { //AATStartRadial //AATFinishRadial targetrange = ((Task[i].AATTargetOffsetRadius+1.0)/2.0); double aatbisector = HalfAngle(Task[i].AATStartRadial, Task[i].AATFinishRadial); if (fabs(AngleLimit180(aatbisector-targetbearing))>90) { // bisector is going away from sector targetbearing = Reciprocal(targetbearing); targetrange = 1.0-targetrange; } if (!AngleInRange(Task[i].AATStartRadial, Task[i].AATFinishRadial, targetbearing,true)) { // Bisector is not within AAT sector, so // choose the closest radial as the target line if (fabs(AngleLimit180(Task[i].AATStartRadial-targetbearing)) <fabs(AngleLimit180(Task[i].AATFinishRadial-targetbearing))) { targetbearing = Task[i].AATStartRadial; } else { targetbearing = Task[i].AATFinishRadial; } } targetrange*= Task[i].AATSectorRadius; } else { targetrange = Task[i].AATTargetOffsetRadius *Task[i].AATCircleRadius; } // TODO accuracy: if i=awp and in sector, range parameter needs to // go from current aircraft position to projection of target // out to the edge of the sector if (InAATTurnSector(longitude, latitude, i, altitude) && (awp==i) && !Task[i].AATTargetLocked) { // special case, currently in AAT sector/cylinder double dist; double qdist; double bearing; // find bearing from last target through current aircraft position with offset DistanceBearing(Task[i-1].AATTargetLat, Task[i-1].AATTargetLon, latitude, longitude, &qdist, &bearing); bearing = AngleLimit360(bearing+Task[i].AATTargetOffsetRadial); dist = ((Task[i].AATTargetOffsetRadius+1)/2.0)* FindInsideAATSectorDistance(latitude, longitude, i, bearing); // if (dist+qdist>aatdistance.LegDistanceAchieved(awp)) { // JMW: don't prevent target from being closer to the aircraft // than the best achieved, so can properly plan arrival time FindLatitudeLongitude (latitude, longitude, bearing, dist, &Task[i].AATTargetLat, &Task[i].AATTargetLon); UpdateTargetAltitude(Task[i]); TargetModified = true; // } } else { FindLatitudeLongitude (WayPointList[Task[i].Index].Latitude, WayPointList[Task[i].Index].Longitude, targetbearing, targetrange, &Task[i].AATTargetLat, &Task[i].AATTargetLon); UpdateTargetAltitude(Task[i]); TargetModified = true; } } } CalculateAATIsoLines(); if (!TargetDialogOpen) { TargetModified = false; // allow target dialog to detect externally changed targets } UnlockTaskData(); }