Revision 4dbf0ec391b877f21402aed9e8351fe8f7468d14 authored by D019 Rig on 19 December 2019, 23:25:22 UTC, committed by D019 Rig on 19 December 2019, 23:25:22 UTC
1 parent 4cac1d4
pertmgr.c
//=====================================================================================================================
//
// pertmgr.c : READCXDATA helper functions -- reproduction of Maestro perturbation waveforms modulating a trial target
// trajectory.
//
// AUTHOR: saruffner
//
// DESCRIPTION:
// Maestro/MaestroDRIVER supports the perturbation of trial target trajectories via the application of one of several
// kinds of perturbation waveforms, defined by the TARGET_PERTURB trial code group. This module encapsulates the
// details of processing the TARGET_PERTURB trial codes and calculating the contributions of any defined perturbations
// on a tick-by-tick basis. It essentially reproduces the code from the class CCxPertHelper, which is part of
// MaestroDRIVER's code base. It also includes the uniform and Gaussian random number generators that implement the
// PERT_ISNOISE and PERT_ISGAUSS perturbation types.
//
// REVISION HISTORY:
// 29jul2005-- Began development.
// 16jul2007-- Added support for perturbing target window or pattern speed (PERT_ON_SWIN, PERT_ON_SPAT), which was
// introduced in Maestro 2.1.2.
// 06sep2007-- Added support for perturing target window AND pattern speed at the same time (PERT_ON_SPD), or target
// window and pattern direction at the same time (PERT_ON DIR). These were introduced in Maestro 2.1.3.
//=====================================================================================================================
#include "math.h" // for various math/trig functions
#ifndef M_PI // this is defined on UNIX/Linux/Mac, but not Windows
#define M_PI 3.14159265359
#endif
#include "pertmgr.h"
#define TORADIANS(D) (((double)D) * M_PI / 180.0)
//=====================================================================================================================
// MODULE-PRIVATE FUNCTION PROTOTYPES
//=====================================================================================================================
double computePert( int iTime, PMPERTOBJ pPert );
//=====================================================================================================================
// FUNCTIONS FOR RANDOM NUMBER GENERATION
//
// These functions and the corresponding state information essentially reproduce the functionality of Maestro CXDRIVER
// classes CUniformRNG and CGaussRNG defined in cxdriver/utilities.cpp. The header comments for those two classes are
// copied below...
//
// Implementation of class CUniformRNG
// -----------------------------------
// This class encapsulates a pseudo-random number generator that returns a sequence of uniformly distributed floating-
// point values in (0.0 .. 1.0), endpoints excluded. It encapsulates the "ran1" algorithm presented on p.282 in:
// Press, WH; et al. "Numerical recipes in C: the art of acientific computing". New York: Cambridge University
// Press, Copyright 1988-1992.
//
// The algorithm uses a 32-entry table to shuffle the output of a "Minimal Standard" linear congruential generator, of
// the form I(n+1) = A*I(n) % M (with A and M carefully chosen). Schrage's method is used to compute I(n+1) without
// an integer overflow. The 32-bit integers output by the algorithm fall in the range [1..M-1]; dividing by M=2^31
// gives a double-valued output in (0..1). For algorithmic details, consult the book. There are few comments here.
//
// Portability note: We assume here that int is 32-bit!!
//
// IAW the licensing policy of "Numerical Recipes in C", this class is not distributable in source code form without
// obtaining the appropriate license; however, it may appear in an executable file that is distributed.
//
// Implementation of class CGaussRNG
// ---------------------------------
// This class encapsulates a pseudo-random number generator that returns a sequence of normally distributed floating-
// point values with zero mean and unit variance. It encapsulates the "gasdev" algorithm presented on p.289 in:
// Press, WH; et al. "Numerical recipes in C: the art of acientific computing". New York: Cambridge University
// Press, Copyright 1988-1992.
//
// The algorithm uses the polar form of the Box-Muller transformation to transform a sequence of uniform deviates to
// a sequence of Gaussian deviates. We use CUniformRNG as the source of the uniform sequence. For algorithmic
// details, consult the book.
//
// IAW the licensing policy of "Numerical Recipes in C", this class is not distributable in source code form without
// obtaining the appropriate license; however, it may appear in an executable file that is distributed.
//=====================================================================================================================
VOID seedUniformRNG( PUNIFORMRNG pUnif, int seed )
{
int j, k;
pUnif->curr = (seed == 0) ? 1 : ((seed < 0) ? -seed : seed); // start at strictly positive seed value
for( j = URNG_TABLESZ+7; j >= 0; j-- ) // after discarding first 8 integers generated
{ // by the algorithm, fill shuffle table with
k = pUnif->curr/URNG_Q; // next TABLESZ integers generated
pUnif->curr = URNG_A * (pUnif->curr - k*URNG_Q) - k*URNG_R;
if( pUnif->curr < 0 ) pUnif->curr += URNG_M;
if( j < URNG_TABLESZ ) pUnif->shuffle[j] = pUnif->curr;
}
pUnif->lastOut = pUnif->shuffle[0];
}
double getUniformRNG( PUNIFORMRNG pUnif )
{
int k, index;
k = pUnif->curr/URNG_Q; // compute I(n+1) = A*I(n) % M using Schrage's method
pUnif->curr = URNG_A * (pUnif->curr - k*URNG_Q) - k*URNG_R; // to avoid integer overflows
if( pUnif->curr < 0 ) pUnif->curr += URNG_M;
index = pUnif->lastOut / URNG_NDIV; // use last # retrieved from shuffle table to calc
pUnif->lastOut = pUnif->shuffle[index]; // index of next # to retrieve. Replace that entry in
pUnif->shuffle[index] = pUnif->curr; // shuffle table with curr output of LC generator
return( URNG_DSCALE * pUnif->lastOut ); // convert int in [1..M-1] to double in (0..1)
}
VOID seedGaussRNG( PGAUSSRNG pGauss, int seed )
{
seedUniformRNG( &(pGauss->uniformRNG), seed );
pGauss->bGotNext = FALSE;
}
double getGaussRNG( PGAUSSRNG pGauss )
{
double dVal = 0;
double v1, v2, rsq, fac;
if( !pGauss->bGotNext )
{
do // get two uniform deviates v1,v2 such that (v1,v2)
{ // lies strictly inside the unit circle, but not at
v1 = 2.0 * getUniformRNG( &(pGauss->uniformRNG) ) - 1.0; // the origin
v2 = 2.0 * getUniformRNG( &(pGauss->uniformRNG) ) - 1.0;
rsq = v1*v1 + v2*v2;
} while( rsq >= 1.0 || rsq == 0.0 );
fac = sqrt( -2.0 * log(rsq) / rsq ); // use Box-Muller transformation to transform the
pGauss->dNext = v1*fac; // the uniform deviates to two Gaussian deviates, one
pGauss->bGotNext = TRUE; // of which is saved for next call to this function!
dVal = v2*fac;
}
else
{
dVal = pGauss->dNext;
pGauss->bGotNext = FALSE;
}
return( dVal );
}
//=====================================================================================================================
// FUNCTIONS FOR THE PERTURBATION MANAGER
//
// These functions duplicate similarly named functions in the MaestroDRIVER class CCxPertHelper. Each function has
// a header **copied from the CCxPertHelper source file**. You can think of the PERTMGR pointer argument in these
// methods as roughly equivalent to the hidden 'this' pointer in C++.
//=====================================================================================================================
//=== Reset ===========================================================================================================
//
// Remove all currently defined perturbations. Release any random number generators that were created to implement
// noise perturbations.
//
// ARGS: NONE.
// RETURNS: NONE.
//
VOID resetPertManager( PMPERTMGR pPertMgr )
{
pPertMgr->nPerts = 0;
}
//=== ProcessTrialCodes ===============================================================================================
//
// Translates a TARGET_PERTURB trial code set into a new perturbation object to be applied during the trial. The
// index of the affected target, the perturbation's start time within the trial, and the affected target velocity
// component are all included in the trial code set, along with the parameters defining the perturbation itself.
//
// ARGS: pCodes -- [in] ptr to a set of five TRIALCODEs representing a TARGET_PERTURB code group.
//
// RETURNS: TRUE if successful, FALSE otherwise.
//
BOOL processPertCodes( PMPERTMGR pPertMgr, TRIALCODE* pCodes )
{
PMPERTOBJ pNew;
if( pPertMgr->nPerts == MAX_TRIALPERTS || pCodes[0].code != TARGET_PERTURB ) return( FALSE );
pNew = &(pPertMgr->perts[ pPertMgr->nPerts ]);
pNew->iTgt = (int) pCodes[1].code;
pNew->idCmpt = (int) (pCodes[1].time >> 4);
pNew->iStart = (int) pCodes[0].time;
pNew->fAmp = ((float) pCodes[2].code) / 10.0f;
pNew->def.iType = (int) (pCodes[1].time & 0x0F);
pNew->def.iDur = (int) pCodes[2].time;
switch( pNew->def.iType ) // translate type-specific perturbation parameters
{
case PERT_ISSINE :
pNew->def.sine.iPeriod = (int) pCodes[3].code;
pNew->def.sine.fPhase = ((float) pCodes[3].time)/100.0f;
break;
case PERT_ISTRAIN :
pNew->def.train.iPulseDur = (int) pCodes[3].code;
pNew->def.train.iRampDur = (int) pCodes[3].time;
pNew->def.train.iIntv = (int) pCodes[4].code;
break;
case PERT_ISNOISE :
case PERT_ISGAUSS :
pNew->def.noise.iUpdIntv = (int) pCodes[3].code;
pNew->def.noise.fMean = ((float) pCodes[3].time)/1000.0f;
pNew->def.noise.iSeed = (int) MAKELONG(pCodes[4].time, pCodes[4].code);
if( pNew->def.iType == PERT_ISNOISE )
seedUniformRNG( &(pNew->uniformRNG), pNew->def.noise.iSeed );
else
seedGaussRNG( &(pNew->gaussRNG), pNew->def.noise.iSeed );
pNew->dLastRandom = 0.0;
break;
default :
return( FALSE );
}
++(pPertMgr->nPerts);
return( TRUE );
}
//=== Perturb =========================================================================================================
//
// Calculate the offset vectors (DH, DV) that represent the net effect of any perturbations applied to the nominal
// window and pattern velocities of the specified target. If none of the currently defined perturbations affect
// the specified target at the current time, then both offset vectors will be (0,0).
//
// IMPORTANT: By design, the two directional perturbations (PERT_ON_DWIN, PERT_ON_DPAT) rotate the *nominal*
// velocity vector by some random angle. The offset vector returned gives the horizontal and vertical deltas
// necessary to achieve this rotation. Applying a directional and a velocity component or speed perturbation at the
// same time would be rather confusing!
//
// ARGS: iTgt -- [in] index of target in the trial target map.
// iTime -- [in] current trial time in ms.
// fpWin -- [in] current "nominal" window velocity of tgt
// fpPat -- [in] current "nominal" pattern velocity of tgt
// fpPertWin -- [out] net offset in "nominal" window velocity due to any defined perturbations
// fpPertPat -- [out] net offset in "nominal" pattern velocity due to any defined perturbations
//
// RETURNS: NONE.
//
VOID perturbTarget( PMPERTMGR pPertMgr, int iTgt, int iTime, double winVelH, double winVelV, double patVelH,
double patVelV, double* pWinOffsetH, double* pWinOffsetV, double* pPatOffsetH, double* pPatOffsetV )
{
int i;
double dCurr;
double dR, dTheta; // for handling directional perts
PMPERTOBJ pPert;
*pWinOffsetH = 0.0; // make sure net perturbations start at (0,0)!
*pWinOffsetV = 0.0;
*pPatOffsetH = 0.0;
*pPatOffsetV = 0.0;
for( i = 0; i < pPertMgr->nPerts; i++ ) // for each active perturbation...
{
pPert = &(pPertMgr->perts[i]);
if( iTgt == pPert->iTgt ) // ...if pert affects this tgt,
{
dCurr = computePert( iTime, pPert ); // compute the current value of the velocity
if( dCurr == 0.0 ) continue; // or directional pert. if 0, no effect.
switch( pPert->idCmpt ) // if perturbed, update the appropriate offset
{ // vector IAW what trajectory cmpt is perturbed
case PERT_ON_HWIN :
*pWinOffsetH += dCurr;
break;
case PERT_ON_VWIN :
*pWinOffsetV += dCurr;
break;
case PERT_ON_HPAT :
*pPatOffsetH += dCurr;
break;
case PERT_ON_VPAT :
*pPatOffsetV += dCurr;
break;
case PERT_ON_DWIN : // NOTE: the directional pert is a rotation in
dR = sqrt( winVelH*winVelH + winVelV*winVelV ); // in degrees, not radians!
dTheta = atan2( winVelV, winVelH );
dTheta += TORADIANS(dCurr);
*pWinOffsetH += dR * cos(dTheta) - winVelH;
*pWinOffsetV += dR * sin(dTheta) - winVelV;
break;
case PERT_ON_DPAT :
dR = sqrt( patVelH * patVelH + patVelV * patVelV );
dTheta = atan2( patVelV, patVelH );
dTheta += TORADIANS(dCurr);
*pPatOffsetH += dR * cos(dTheta) - patVelH;
*pPatOffsetV += dR * sin(dTheta) - patVelV;
break;
case PERT_ON_SWIN :
dR = sqrt( winVelH*winVelH + winVelV*winVelV );
dR += dCurr;
dTheta = atan2( winVelV, winVelH );
*pWinOffsetH += dR * cos(dTheta) - winVelH;
*pWinOffsetV += dR * sin(dTheta) - winVelV;
break;
case PERT_ON_SPAT :
dR = sqrt( patVelH * patVelH + patVelV * patVelV );
dR += dCurr;
dTheta = atan2( patVelV, patVelH );
*pPatOffsetH += dR * cos(dTheta) - patVelH;
*pPatOffsetV += dR * sin(dTheta) - patVelV;
break;
case PERT_ON_DIR :
dR = sqrt( winVelH*winVelH + winVelV*winVelV );
dTheta = atan2( winVelV, winVelH );
dTheta += TORADIANS(dCurr);
*pWinOffsetH += dR * cos(dTheta) - winVelH;
*pWinOffsetV += dR * sin(dTheta) - winVelV;
dR = sqrt( patVelH * patVelH + patVelV * patVelV );
dTheta = atan2( patVelV, patVelH );
dTheta += TORADIANS(dCurr);
*pPatOffsetH += dR * cos(dTheta) - patVelH;
*pPatOffsetV += dR * sin(dTheta) - patVelV;
break;
case PERT_ON_SPD :
dR = sqrt( winVelH*winVelH + winVelV*winVelV );
dR += dCurr;
dTheta = atan2( winVelV, winVelH );
*pWinOffsetH += dR * cos(dTheta) - winVelH;
*pWinOffsetV += dR * sin(dTheta) - winVelV;
dR = sqrt( patVelH * patVelH + patVelV * patVelV );
dR += dCurr;
dTheta = atan2( patVelV, patVelH );
*pPatOffsetH += dR * cos(dTheta) - patVelH;
*pPatOffsetV += dR * sin(dTheta) - patVelV;
break;
}
}
}
}
//=== Compute =========================================================================================================
//
// Compute value of specified perturbation waveform for the specified trial time.
//
// ARGS: iTime -- [in] trial time in ms.
// pPert -- [in] the perturbation object
//
// RETURNS: perturbation value (either velocity in deg/s or a directional offset in deg)
//
double computePert( int iTime, PMPERTOBJ pPert )
{
int t, t1, t2, t3;
double dVal, dAmp, dTwoPi, dOmega_t, dRad, dSlope, dTime;
if( iTime < pPert->iStart || // perturbation off
iTime >= (pPert->iStart + pPert->def.iDur) )
return( 0.0 );
t = iTime - pPert->iStart; // time since perturbation started
dVal = 0.0; // computed value of perturbation waveform
if( pPert->def.iType == PERT_ISSINE ) // SINE: v(t) = A*sin(2PI*t/T + phi), where A = amp
{ // in deg/s, T = period in ms, phi = phase in deg.
dAmp = (double) pPert->fAmp;
dTwoPi = M_PI * 2.0;
dOmega_t = dTwoPi * ((double) t) / ((double)pPert->def.sine.iPeriod);
dRad = dOmega_t + TORADIANS( pPert->def.sine.fPhase );
while( dRad >= dTwoPi ) dRad -= dTwoPi;
dVal = dAmp * sin( dRad );
}
else if( pPert->def.iType == PERT_ISTRAIN ) // TRAIN: Let D= pulse dur(ms), I= intv (ms),
{ // R= ramp dur(ms), and A = pulse amp(deg/s).
t = t % pPert->def.train.iIntv; // t' = time within a pulse presentation
t1 = pPert->def.train.iRampDur; // end of acceleration phase
t2 = t1 + pPert->def.train.iPulseDur; // end of constant-velocity phase
t3 = t2 + pPert->def.train.iRampDur; // end of deceleration phase
dSlope = ((double)pPert->fAmp) * 1000.0; // ramp "slope" = A/(R/1000) in deg/sec^2
dSlope /= (double) pPert->def.train.iRampDur;
dTime = ((double) t) / 1000.0; // t' converted from ms --> sec
if( t >= 0 && t < t1 ) dVal = dSlope * dTime; // for t' in [0..R), v(t') = slope * t'.
else if( t >= t1 && t < t2 ) dVal = ((double) pPert->fAmp); // for t' in [R..R+D), v(t') = A.
else if( t >= t2 && t < t3 ) // for t' in [R+D..2R+D),
dVal = dSlope * (((double)t3)/1000.0 - dTime); // v(t') = slope * (2R+D-t' in sec)
}
else if( pPert->def.iType == PERT_ISNOISE ) // Uniform NOISE: Steplike waveform changes once per
{ // update interval.
if( t % pPert->def.noise.iUpdIntv == 0 )
{
dVal = 2.0*getUniformRNG( &(pPert->uniformRNG) ) - 1.0; // 2*U(0..1) - 1 ==> U(-1..1)
dVal += (double) pPert->def.noise.fMean; // U(-1_mean .. 1+mean)
dVal *= (double) pPert->fAmp; // U(-1+mean .. 1+mean)*amplitude
pPert->dLastRandom = dVal; // remember value until next update
}
else
dVal = pPert->dLastRandom;
}
else if( pPert->def.iType == PERT_ISGAUSS ) // Gaussian NOISE: Steplike waveform changes once per
{ // update interval.
if( t % pPert->def.noise.iUpdIntv == 0 )
{
dVal = getGaussRNG( &(pPert->gaussRNG) ); // N(0,1) [Gaussian w/zero mean and 1 std dev]
dVal *= (double) pPert->fAmp; // N(0,amplitude)
dVal += (double) (pPert->def.noise.fMean * pPert->fAmp); // N(mean*amplitude, amplitude)
pPert->dLastRandom = dVal; // remember value until next update
}
else
dVal = pPert->dLastRandom;
}
return( dVal );
}
Computing file changes ...
