128 lines
3.6 KiB
C
128 lines
3.6 KiB
C
#include <math.h>
|
|
#include <stdlib.h>
|
|
#include "mc_ext.h"
|
|
|
|
/*$**************************************
|
|
NAME : MC_Tracks_Prio_SortTracks
|
|
INPUTS :
|
|
InputTrack1 : TMissionTrack
|
|
InputTrack2 : TMissionTrack
|
|
InputTrack3 : TMissionTrack
|
|
InputTrack4 : TMissionTrack
|
|
OUPUTS :
|
|
OutputTrack1 : TMissionTrack
|
|
OutputTrack2 : TMissionTrack
|
|
OutputTrack3 : TMissionTrack
|
|
OutputTrack4 : TMissionTrack
|
|
***************************************$*/
|
|
|
|
void mc_tracks_prio_sorttracks(
|
|
const TMissionTrack *InputTrack1, const TMissionTrack *InputTrack2,
|
|
const TMissionTrack *InputTrack3, const TMissionTrack *InputTrack4,
|
|
mc_tracks_prio_sorttracks_out *out)
|
|
{
|
|
TMissionTrack _LO1_newA = *InputTrack1;
|
|
TMissionTrack _LO1_newB = *InputTrack1;
|
|
TMissionTrack _LO2_newA = *InputTrack1;
|
|
TMissionTrack _LO2_newB = *InputTrack1;
|
|
TMissionTrack _LO3_newA = *InputTrack1;
|
|
TMissionTrack _LO3_newB = *InputTrack1;
|
|
TMissionTrack _LO4_newA = *InputTrack1;
|
|
TMissionTrack _LO4_newB = *InputTrack1;
|
|
TMissionTrack _LO5_newA = *InputTrack1;
|
|
TMissionTrack _LO5_newB = *InputTrack1;
|
|
TMissionTrack _LO6_newA = *InputTrack1;
|
|
TMissionTrack _LO6_newB = *InputTrack1;
|
|
|
|
TMissionTrack _LI_A = *InputTrack1;
|
|
TMissionTrack _LI_B = *InputTrack2;
|
|
|
|
SortBlockPriorities(&_LI_A, &_LI_B, &_LO4_newA, &_LO4_newB);
|
|
|
|
_LI_A = *InputTrack3;
|
|
_LI_B = *InputTrack4;
|
|
SortBlockPriorities(&_LI_A, &_LI_B, &_LO6_newA, &_LO6_newB);
|
|
|
|
SortBlockPriorities(&_LO4_newB, &_LO6_newA, &_LO2_newA, &_LO2_newB);
|
|
|
|
SortBlockPriorities(&_LO4_newA, &_LO2_newA, &_LO1_newA, &_LO1_newB);
|
|
|
|
out->OutputTrack1 = _LO1_newA;
|
|
|
|
SortBlockPriorities(&_LO2_newB, &_LO6_newB, &_LO5_newA, &_LO5_newB);
|
|
|
|
SortBlockPriorities(&_LO1_newB, &_LO5_newA, &_LO3_newA, &_LO3_newB);
|
|
|
|
out->OutputTrack2 = _LO3_newA;
|
|
out->OutputTrack3 = _LO3_newB;
|
|
out->OutputTrack4 = _LO5_newB;
|
|
}
|
|
|
|
/* ROLE :,
|
|
Sort two mission tracks according to:,
|
|
1) their (rate of closing / distance) ratio,
|
|
2) target type,
|
|
3) detection or not by the Radar */
|
|
void SortBlockPriorities(const TMissionTrack *InputTrackA, const TMissionTrack *InputTrackB, TMissionTrack *OutputTrackA, TMissionTrack *OutputTrackB)
|
|
{
|
|
bool bInvertTracks = false;
|
|
real vrDivDResultTrackA = 0.0;
|
|
real vrDivDResultTrackB = 0.0;
|
|
|
|
vrDivDResultTrackA = CalculateVrDivD(InputTrackA->Vr, InputTrackA->D);
|
|
vrDivDResultTrackB = CalculateVrDivD(InputTrackB->Vr, InputTrackB->D);
|
|
|
|
bInvertTracks = (InputTrackA->targetType == TTargetType_FRIEND);
|
|
bInvertTracks = bInvertTracks || !(InputTrackA->detectedByRadar);
|
|
if ( ( fabs(vrDivDResultTrackA) < 0.0001 ) && ( fabs(vrDivDResultTrackB) < 0.0001 ) ) {
|
|
bInvertTracks = bInvertTracks ||
|
|
( (InputTrackA->detectedByRadar) &&
|
|
(InputTrackB->detectedByRadar) &&
|
|
( InputTrackA->D > InputTrackB->D ) );
|
|
|
|
} else {
|
|
bInvertTracks = bInvertTracks ||
|
|
( (InputTrackA->detectedByRadar) &&
|
|
(InputTrackB->detectedByRadar) &&
|
|
(vrDivDResultTrackA < vrDivDResultTrackB) );
|
|
}
|
|
|
|
if (bInvertTracks) {
|
|
*OutputTrackA = *InputTrackB;
|
|
*OutputTrackB = *InputTrackA;
|
|
} else {
|
|
*OutputTrackA = *InputTrackA;
|
|
*OutputTrackB = *InputTrackB;
|
|
}
|
|
}
|
|
|
|
/* ROLE :,
|
|
Calculate: result = rate of closing / distance */
|
|
real CalculateVrDivD(const float _I0_Vr, const float _I1_D)
|
|
{
|
|
bool bDIsNotZero = (_I1_D > 0.1);
|
|
|
|
if (bDIsNotZero) {
|
|
return ( _I0_Vr / _I1_D ) ;
|
|
} else {
|
|
return ( 0.0 );
|
|
}
|
|
}
|
|
|
|
void rand_step(rand_out *out)
|
|
{
|
|
float a = (float)(rand());
|
|
kcg_real b = (float)RAND_MAX;
|
|
out->o = a/b;
|
|
}
|
|
|
|
void int_of_float_step(float a, int_of_float_out *out)
|
|
{
|
|
return (int) a;
|
|
}
|
|
|
|
void float_of_int_step(int a, int_of_float_out *out)
|
|
{
|
|
return (float) a;
|
|
}
|