Point2F BiQuadToSqr::transform( const Point2F &p ) const
{
Point2F kA = m_kP00 - p;
F32 fAB = mDotPerp( kA, m_kB );
F32 fAC = mDotPerp( kA, m_kC);
// 0 = ac*bc+(bc^2+ac*bd-ab*cd)*s+bc*bd*s^2 = k0 + k1*s + k2*s^2
F32 fK0 = fAC*m_fBC;
F32 fK1 = m_fBC*m_fBC + fAC*m_fBD - fAB*m_fCD;
F32 fK2 = m_fBC*m_fBD;
if (mFabs(fK2) > POINT_EPSILON)
{
// s-equation is quadratic
F32 fInv = 0.5f/fK2;
F32 fDiscr = fK1*fK1 - 4.0f*fK0*fK2;
F32 fRoot = mSqrt( mFabs(fDiscr) );
Point2F kResult0( 0, 0 );
kResult0.x = (-fK1 - fRoot)*fInv;
kResult0.y = fAB/(m_fBC + m_fBD*kResult0.x);
F32 fDeviation0 = deviation(kResult0);
if ( fDeviation0 == 0.0f )
return kResult0;
Point2F kResult1( 0, 0 );
kResult1.x = (-fK1 + fRoot)*fInv;
kResult1.y = fAB/(m_fBC + m_fBD*kResult1.x);
F32 fDeviation1 = deviation(kResult1);
if ( fDeviation1 == 0.0f )
return kResult1;
if (fDeviation0 <= fDeviation1)
{
if ( fDeviation0 < POINT_EPSILON )
return kResult0;
}
else
{
if ( fDeviation1 < POINT_EPSILON )
return kResult1;
}
}
else
{
// s-equation is linear
Point2F kResult( 0, 0 );
kResult.x = -fK0/fK1;
kResult.y = fAB/(m_fBC + m_fBD*kResult.x);
F32 fDeviation = deviation(kResult);
if ( fDeviation < POINT_EPSILON )
return kResult;
}
// point is outside the quadrilateral, return invalid
return Point2F(F32_MAX,F32_MAX);
}
void AdaptivePaddedNoZeroDevAverage::sample(float new_sample) {
// Compute our parent classes sample information
AdaptiveWeightedAverage::sample(new_sample);
float new_avg = average();
if (new_sample != 0) {
// We only create a new deviation if the sample is non-zero
float new_dev = compute_adaptive_average(fabsd(new_sample - new_avg),
deviation());
set_deviation(new_dev);
}
set_padded_average(new_avg + padding() * deviation());
_last_sample = new_sample;
}
double check(double ****f) //give energy of pulsations
{
double PulsEnergy=0;
double **averf;
int n=max(n1,max(n2,n3));
TotalEnergy=0;
averf = alloc_mem_2f(3,n);
int i,j,k,l;
for(l=0;l<=2;l++)
for(i=0;i<n;i++)
averf[l][i] = 0;
for(i=ghost;i<mm1;i++)
for(j=ghost;j<mm2;j++)
for(k=ghost;k<mm3;k++)
{
PulsEnergy+=deviation(f,i,j,k);
for(l=0;l<=2;l++) averf[l][k-ghost] += f[l][i][j][k];
}
for(l=0;l<=2;l++)
for(k=ghost;k<mm3;k++)
{
// averf[l][k-ghost] /= n1*n2;
// for(i=ghost;i<mm1;i++)
// for(j=ghost;j<mm2;j++)
TotalEnergy += pow(averf[l][k-ghost],2.);
}
TotalEnergy += 1.; //if zero average field
razlet = (PulsEnergy/TotalEnergy>UpLimit);
free_mem_2f(averf,3,n);
return(PulsEnergy);
}
void check(double ****f) //calculate energy of pulsations of all components and know if there's crash
{
int i,j,k,l;
PulsEnergy=0;
TotalEnergy=0;
for(l=0;l<=2;l++)
for(i=0;i<m1;i++)
for(k=0;k<m3;k++)
averf[l][i][k] = 0;
for(i=0;i<m1;i++)
for(j=ghost;j<mm2;j++)
for(k=0;k<m3;k++)
if(isType(node[i][k],NodeFluid) && !isType(node[i][k],NodeClued))
{
PulsEnergy+=deviation(f,i,j,k);
for(l=0;l<=2;l++) averf[l][i][k] += f[l+1][i][j][k];
}
for(l=0;l<=2;l++)
for(i=0;i<m1;i++)
for(k=0;k<m3;k++)
if(isType(node[i][k],NodeFluid) && !isType(node[i][k],NodeClued))
TotalEnergy += pow(averf[l][i][k],2.);
TotalEnergy += 1.; //if zero average field
razlet = (PulsEnergy/TotalEnergy>UpLimit);
}
inline void statistics(std::vector<T>& measurements, T& min, T& max, T& avg, T& med, T& dev)
{
std::sort(measurements.begin(), measurements.end());
min = measurements.front();
max = measurements.back();
avg = mean(measurements);
med = measurements[measurements.size()/2];
dev = deviation(measurements);
}
TSGame::TSGame() : stars {sf::Color::White, 2, 4, sf::Vector2f{0.1,0.1}, sf::Vector2i{5, 10}, sf::Vector2f{400.f, 300.f}, 400.f, sf::Vector2f{0.1f, 0.1f}, 80.f, sf::Vector2f{0.f, 360.f}}
{
level = 0;
levelCounter = 0;
Score = 0.f;
fpsLimit = 200;
mainFont.loadFromFile("resources/DejaVuSans.ttf");
textOverlay.setPosition(5,5);
textOverlay.setFont(mainFont);
textOverlay.setCharacterSize(12);
playerSize = 10.f;
player.setRadius(10.f);
player.setPointCount(6);
player.setOrigin(sf::Vector2f {playerSize,playerSize});
player.setFillColor(sf::Color {80,80,80,255});
mainMusic.setLoop(true);
mainMusic.setVolume(50.f);
mainMusic.openFromFile("resources/TriangleSuper.ogg");
mainMusic.play();
gameStatus = true;
scoreClock.restart();
enemyAISine =
[](sf::Vector2f currentPosition, sf::Vector2f &startingPoint, sf::Vector2f &Destination, sf::Vector2f &linearPosition, float &angle, float deltaTime)
{
angle += 0.01 * deltaTime;
sf::Vector2f direction = Destination - startingPoint;
normalize(direction);
float sinDeviation = sin(2 * angle) - 0.5;
sf::Vector2f deviation(-direction.y, direction.x);
deviation.x *= sinDeviation * 30.f;
deviation.y *= sinDeviation * 30.f;
linearPosition.x += direction.x * 300.f * deltaTime/1000.f;
linearPosition.y += direction.y * 300.f * deltaTime/1000.f;
return linearPosition + deviation;
};
enemyAILine =
[](sf::Vector2f currentPosition, sf::Vector2f &startingPoint, sf::Vector2f &Destination, sf::Vector2f &linearPosition, float &angle, float deltaTime)
{
sf::Vector2f direction = Destination - startingPoint;
normalize(direction);
linearPosition.x += direction.x * 300.f * deltaTime/1000.f;
linearPosition.y += direction.y * 300.f * deltaTime/1000.f;
return linearPosition;
};
srand(time(NULL));
}
void AdaptivePaddedAverage::sample(float new_sample) {
// Compute new adaptive weighted average based on new sample.
AdaptiveWeightedAverage::sample(new_sample);
// Now update the deviation and the padded average.
float new_avg = average();
float new_dev = compute_adaptive_average(fabsd(new_sample - new_avg),
deviation());
set_deviation(new_dev);
set_padded_average(new_avg + padding() * new_dev);
_last_sample = new_sample;
}
void AdaptivePaddedAverage::sample(double new_sample) {
// Compute our parent classes sample information
AdaptiveWeightedAverage::sample(new_sample);
// Now compute the deviation and the new padded sample
float new_avg = average();
float new_dev = compute_adaptive_average(fabsd(new_sample - new_avg),
deviation());
set_deviation(new_dev);
set_padded_average(new_avg + padding() * new_dev);
_last_sample = new_sample;
}
double similarity(char *text, const LangStats *stats, int ngrams_count) {
double sim = 0;
double dev;
double *freq;
for (int i = 0; i < ngrams_count; i++) {
freq = ngrams_freq(text, i + 1);
dev = deviation(freq, stats->ngrams[i], pow(ALPHABET_LENGTH, i + 1));
sim += (dev / (i + 1));
}
free(freq);
return sim;
}
cv::Mat Normalizer::run(cv::Mat& im, double reqmean, double reqvar)
{
im.convertTo(im, CV_32FC1);
cv::minMaxLoc(im, &min, &max, &min_loc, &max_loc);
mean = cv::mean(im);
normalizedImage = im - mean[0];
cv::Scalar normMean = cv::mean(normalizedImage);
stdNorm = deviation(normalizedImage, normMean[0]);
normalizedImage = normalizedImage / stdNorm;
normalizedImage = reqmean + normalizedImage * cv::sqrt(reqvar);
cv::minMaxLoc(normalizedImage, &minNorm, &maxNorm);
return normalizedImage;
}
void
LPCPLAY::readjust(float maxdev, double *pchval,
float firstframe, float lastframe,
float thresh, float weight)
{
float dev;
dev = deviation(firstframe,lastframe,weight,thresh);
if (!dev)
dev=.0001;
if (maxdev)
{
// If negative, use as factor to multiply orig deviation
if (maxdev < 0)
maxdev = dev * -maxdev;
rtcmix_advise("LPCPLAY", "Adjusting pitch deviation to %f Hz",maxdev);
adjust(dev,maxdev,weight,pchval,firstframe,lastframe);
}
}
void check(double ***f) //calculate energy of pulsations of all components and know if there's crash
{
int i,k,l;
//double meanp = 0;
//long kol=0;
PulsEnergy=0;
TotalEnergy=0;
for(i=0;i<m1;i++)
for(k=0;k<m3;k++)
if(isType(node[i][k],NodeFluid) && !isType(node[i][k],NodeClued))
{
// meanp += f[0][i][k];
// kol++;
PulsEnergy+=deviation(f,i,k);
for(l=1;l<=3;l++) TotalEnergy += fabs(1+coordin(i,0)*rc)*pow(f[l][i][k],2.);
}
/*meanp /= kol;
for(i=0;i<m1;i++)
for(k=0;k<m3;k++)
f[0][i][k] -= meanp;*/
TotalEnergy += 1.; //if zero average field
razlet = (PulsEnergy/TotalEnergy>UpLimit);
}
//------------------------------------------------------------------------------
void InitialFromImage::initializeLocal()
{
double rScale = 0.02;
VEC3 shift;
for(int d=0; d<dim;d++)
shift[d] = domain.dom[d][1] - domain.dom[d][0];
VEC3 v;
VEC3 rx;
VEC3 rz;
vector<VEC3> _r;
_r.push_back( VEC3() );
int id = 0;
switch(dim){
case 3:
if(gType == gridType::FCC){
_r.push_back( VEC3() );
_r.push_back( VEC3() );
}
if(gType == gridType::FCC)
{
rx = {0.5*spacing, 0.5*spacing, 0};
rz = {0, 0.5*spacing, 0.5*spacing};
}
for(int i=0; i<nXYZ[0]; i++)
{
for(int j=0; j<nXYZ[1]; j++)
{
for(int k=0; k<nXYZ[2]; k++)
{
arma::vec deviation = deviationS0*arma::randn(dim + 1);
double s = s0*(1 + deviation(0));
v = {0, 0, 0};
_r[0] = { spacing*(i + 0.25 + rScale * deviation(1))
, spacing*(j + 0.25 + rScale * deviation(2))
, spacing*(k + 0.25 + rScale * deviation(3)) };
if(gType == gridType::FCC)
{
_r[1] = _r[0] + rx;
_r[2] = _r[0] + rz;
}
for(VEC3 r:_r)
{
r = periodicBoundary( r, shift );
// Adding particles to the correct domain
#ifdef USE_MPI
if( BLOCK_OWNER(getGridId(r), nNodes, totalNumberOfgridpoints) == myRank )
{
Particle *P = new Particle(id, rho, s, volume, r, r, v);
particles.push_back(P);
}
#else
Particle *P = new Particle(id, rho, s, volume, r, r, v);
particles.push_back(P);
#endif
id++;
}
}
}
}
break;
default:
cerr << "dim =" << dim << " not implemented for coniguration:' Box'" << endl;
exit(EXIT_FAILURE);
break;
}
domain.setParticles(particles);
domain.update();
// exit(EXIT_FAILURE);
}
/*! Propagate a loitering trajectory, filling the lists of sampleTimes
* and samples with the results of each step. Errors during propagation
* are indicated by setting the error flag in the PropagationFeedback
* object.
*
* The return value of propagate is the final state after propagation
* is complete (i.e. the last entry in the samples list.)
*/
sta::StateVector
ScenarioLoiteringTrajectory::propagate(PropagationFeedback& propFeedback,
const sta::StateVector& initialState,
QList<double>& sampleTimes,
QList<sta::StateVector>& samples)
{
QTextStream out (stdout);
double mu = centralBody()->mu();
const ScenarioExtendedTimeline* timeline = simulationParameters()->timeline();
// Creating the list of perturbations that will influence the propagation
ScenarioSpaceVehicle* spacevehicle = dynamic_cast<ScenarioSpaceVehicle*>(this->parent()->parent());
ScenarioProperties* vehicleproperties = spacevehicle->properties();
QList<Perturbations*> perturbationsList = environment()->createListPerturbations(vehicleproperties);
double timelineDuration = sta::daysToSecs(timeline->endTime() - timeline->startTime());
double dt = trajectoryPropagation()->timeStep();
if (dt == 0.0)
{
propFeedback.raiseError(QObject::tr("Time step is zero!"));
return initialState;
}
// We don't output values at every integration step. Instead use the time step
// from simulation parameters. The actual output step used will not necessarily
// match the requested output step: the code below sets it to be an integer
// multiple of the integration step.
double requestedOutputTimeStep = simulationParameters()->timeline()->timeStep();
double outputTimeStep;
unsigned int outputRate;
if (requestedOutputTimeStep < dt)
{
outputRate = 1;
outputTimeStep = dt;
}
else
{
outputRate = (unsigned int) floor(requestedOutputTimeStep / dt + 0.5);
outputTimeStep = outputRate * dt;
}
if (timelineDuration / outputTimeStep > MAX_OUTPUT_STEPS)
{
propFeedback.raiseError(QObject::tr("Number of propagation steps exceeds %1. Try increasing the simulation time step.").arg(MAX_OUTPUT_STEPS));
return initialState;
}
// Calculate initial keplerian elements in case the propagator Two Body will be used
sta::KeplerianElements foundKeplerianElements = cartesianTOorbital(mu, initialState);
double sma = foundKeplerianElements.SemimajorAxis;
double e = foundKeplerianElements.Eccentricity;
double inclination = foundKeplerianElements.Inclination;
double raan = foundKeplerianElements.AscendingNode;
double argOfPeriapsis = foundKeplerianElements.ArgumentOfPeriapsis;
double meanAnomaly = foundKeplerianElements.MeanAnomaly;
double perigee = sma * (1-e);
if (perigee<centralBody()->meanRadius())
{
QMessageBox::warning(NULL, QObject::tr("The trajectory has been not propagated"),
QObject::tr("The perigee distance is smaller than the main body radius."));
return initialState.zero();
}
sta::StateVector stateVector = initialState;
// deviation, reference, and q will be used only in Encke propagation
sta::StateVector deviation(Vector3d::Zero(), Vector3d::Zero());
sta::StateVector reference = initialState;
double q = 0.0;
sampleTimes << timeline->startTime();
samples << stateVector;
double time = timeline->startTime(); //mjd
QFile ciccio("data/PerturbationsData.stae");
QTextStream cicciostream(&ciccio);
ciccio.open(QIODevice::WriteOnly);
unsigned int steps = 0;
for (double t = dt; t < timelineDuration + dt; t += dt)
{
JulianDate jd = timeline->startTime() + sta::secsToDays(t);
// Choosing the propagator and propagating the trajectory
if (trajectoryPropagation()->propagator() == "TWO BODY")
{
double perigee=sma*(1-e);
if (perigee<centralBody()->meanRadius())
{
QMessageBox::warning(NULL,
QObject::tr("The trajectory has been not propagated"),
QObject::tr("The perigee distance is smaller than the main body radius."));
return stateVector.zero();
}
double argOfPeriapsisUpdated = 0.0;
double meanAnomalyUpdated = 0.0;
double raanUpdated = 0.0;
stateVector = propagateTWObody(mu, sma, e, inclination, argOfPeriapsis, raan, meanAnomaly,
trajectoryPropagation()->timeStep(), raanUpdated, argOfPeriapsisUpdated,
meanAnomalyUpdated);
argOfPeriapsis = argOfPeriapsisUpdated;
meanAnomaly = meanAnomalyUpdated;
raan = raanUpdated;
}
else if (trajectoryPropagation()->propagator() == "COWELL")
{
stateVector = propagateCOWELL(mu, stateVector, trajectoryPropagation()->timeStep(), perturbationsList, time, trajectoryPropagation()->integrator(), propFeedback);
}
else if (trajectoryPropagation()->propagator() == "ENCKE")
{
deviation = propagateENCKE(mu, reference, trajectoryPropagation()->timeStep(),perturbationsList, time, stateVector, deviation, q, trajectoryPropagation()->integrator(), propFeedback);
// PropagateTWObody is used to propagate the reference trajectory
double argOfPeriapsisUpdated = 0.0;
double meanAnomalyUpdated = 0.0;
double raanUpdated = 0.0;
reference = propagateTWObody(mu, sma, e, inclination, argOfPeriapsis, raan, meanAnomaly,
trajectoryPropagation()->timeStep(), raanUpdated, argOfPeriapsisUpdated,
meanAnomalyUpdated);
argOfPeriapsis = argOfPeriapsisUpdated;
meanAnomaly = meanAnomalyUpdated;
raan = raanUpdated;
// Calculating the perturbed trajectory
stateVector = reference + deviation;
q = deviation.position.dot(reference.position + 0.5 * deviation.position) / pow(reference.position.norm(), 2.0);
// // Rectification of the reference trajectory, when the deviation is too large.
// if (q > 0.01)
// {
// sta::KeplerianElements keplerian = cartesianTOorbital(mu, stateVector);
//
// sma = keplerian.SemimajorAxis;
// e = keplerian.Eccentricity;
// inclination = keplerian.Inclination;
// argOfPeriapsis = keplerian.ArgumentOfPeriapsis;
// raan = keplerian.AscendingNode;
// meanAnomaly = keplerian.MeanAnomaly;
//
// q = 0;
// reference = stateVector;
// deviation = sta::StateVector(null, null);
// }
}
else if (trajectoryPropagation()->propagator() == "GAUSS")
{
stateVector = propagateGAUSS(mu, stateVector, trajectoryPropagation()->timeStep(), perturbationsList, time, trajectoryPropagation()->integrator());
}
KeplerianElements kep = cartesianTOorbital(mu, stateVector);
cicciostream.setRealNumberPrecision(8);
//cicciostream << kep.SemimajorAxis << " ";
//cicciostream << kep.Eccentricity << " ";
cicciostream << kep.Inclination << " ";
//cicciostream << kep.ArgumentOfPeriapsis << " ";
//cicciostream << kep.AscendingNode << " ";
//cicciostream << kep.TrueAnomaly << " ";
// //Treat the debris perturbation if selected by the user
// foreach (Perturbations* perturbation, perturbationsList)
// if (dynamic_cast<DebrisPerturbations*>(perturbation))
// {
// DebrisPerturbations* debris = dynamic_cast<DebrisPerturbations*>(perturbation);
// double gravityAcceleration = (-pow(initialState.position.norm(),-3.0) * mu * initialState.position).norm();
// double perturbedAcceleration = gravityAcceleration + debris->calculatePerturbingEffect(initialState, sta::daysToSecs(jd));
// }
// Append a trajectory sample every outputRate integration steps (and
// always at the last step.)
if (steps % outputRate == 0 || t >= timelineDuration)
{
sampleTimes << jd;
samples << stateVector;
}
++steps;
time += sta::secsToDays(dt);
};
//out << "samples size: " << samples.size() << endl;
return stateVector;
}
int main() {
#ifdef _DEBUG
int dbgFlags = _CrtSetDbgFlag(_CRTDBG_REPORT_FLAG);
dbgFlags |= _CRTDBG_CHECK_ALWAYS_DF; // check block integrity on every memory call
dbgFlags |= _CRTDBG_DELAY_FREE_MEM_DF;
dbgFlags |= _CRTDBG_LEAK_CHECK_DF; // checks for leaks at process
_CrtSetDbgFlag(dbgFlags);
#endif
printf("Stats, (c) 2016 Vasilisa Petrushina\n");
printf("Enter the number of samples followed by those samples.\n\n");
int ch = getchar();
if (ch != EOF)
ungetc(ch, stdin);
else {
printf("N = 0\nEmpty data set!\n");
return 1;
}
ListLength listLength;
if (!scanf("%llu", &listLength) || listLength < 1) {
printf("N = 0\nEmpty data set!\n");
return 1;
}
double* list = malloc((size_t)listLength * sizeof(double));
if (list == NULL) {
printf("Memory allocation failed!");
return 1;
}
for (unsigned i = 0; i < listLength; ++i)
scanf("%lf", &list[i]);
heapSort(list, listLength);
printf("\nResults\n");
printf("N = %s\n", formatNumber((double)listLength, 0));
printf("Min = %s\n", formatNumber(minimum(list), 3));
printf("Max = %s\n", formatNumber(maximum(list, listLength), 3));
double meanVal = mean(list, listLength);
printf("Arithmetic mean = %s\n", formatNumber(meanVal, 3));
double medianVal = median(list, listLength);
printf("Statistical median = %s\n", formatNumber(medianVal, 3));
double* modeArray = mode(list, listLength);
// check the first element in modeArray indicating how many modes
if (modeArray[0] == 0)
printf("Mode = no mode.\n\n");
else if (modeArray[0] == 1)
printf("Mode = { %s }\n", formatNumber(modeArray[1], 3));
else {
printf("Mode = {");
for (unsigned i = 1; i <= modeArray[0]; ++i) {
printf(" %s", formatNumber(modeArray[i], 3));
if (i != modeArray[0])
printf(", ");
}
printf(" }\n");
}
printf("Mean absolute deviation = %s\n", formatNumber(absoluteDeviation(list, listLength, meanVal), 3));
printf("Median absolute deviation = %s\n", formatNumber(absoluteDeviation(list, listLength, medianVal), 3));
// if first element in mode array (mode count) is not 1, there's no unique mode
if (modeArray[0] != 1.0)
printf("Mode absolute deviation = N/A <no unique mode>\n");
else
printf("Mode absolute deviation = %s\n", formatNumber(absoluteDeviation(list, listLength, modeArray[1]), 3));
free(modeArray);
double varianceVal = variance(list, listLength, meanVal);
printf("Variance = %s\n", formatNumber(varianceVal, 3));
printf("Standard deviation = %s\n\n", formatNumber(deviation(varianceVal), 3));
// frequency distribution table
double interval = (list[listLength - 1] - list[0]) / 10;
// making intervals larger than just range/10 by 1% (to include the higest value) but no more than 0.1
double offset = 0.01 * interval;
if (offset > 0.1)
offset = 0.1;
interval += offset;
double lowerLimit = list[0];
double upperLimit = list[0] + interval;
unsigned index = 0;
// calculations for dynamic allign of collumns
double longestNum = maximum(list, listLength);
if (fabs(minimum(list)) > maximum(list, listLength) )
longestNum = fabs(minimum(list));
int colWidthInterval = countColWidth(longestNum, 2);
int colWidthsCount = countColWidth((double)listLength, 0);
// for each of the 10 intervals count how many numbers get in this range
for (int i = 0; i < 10; ++i) {
unsigned count = 0;
while (list[index] < upperLimit && index < listLength) {
++count;
++index;
}
printf("[%*s..%*s) =%*d : %.2f\n", colWidthInterval, formatNumber(lowerLimit, 2), colWidthInterval, formatNumber(upperLimit, 2), colWidthsCount, count, (double)count / listLength);
lowerLimit = upperLimit;
upperLimit += interval;
}
free(list);
}
void GazeComponent::headShake(int axis, float extent, int count, float speed, float duration)
{
//get head position
h3dFindNodes( m_hID, Config::getParamS( Agent_Param::HeadName_S ), H3DNodeTypes::Joint );
int head_hID = h3dGetNodeFindResult(0);
if(head_hID <= 0)
return;
const float *head_absArray;
h3dGetNodeTransMats( head_hID, 0, &head_absArray );
Vec3f head_pos(head_absArray[12], head_absArray[13], head_absArray[14]);
//check parameters
if(count < 0) count = Config::getParamI(Agent_Param::DfltHeadShakeReps_I);
if(extent < 0) extent = Config::getParamF(Agent_Param::DfltHeadShakeExt_F);
if(speed < 0) speed = Config::getParamF(Agent_Param::DfltHeadShakeSpd_F);
if(duration < 0) duration = Config::getParamF(Agent_Param::DfltHeadShakeDur_F);
if(axis < 0 || axis > 2) axis = Config::getParamI(Agent_Param::DfltHeadShakeAxis_I);
Vec3f deviation(0,0,0);
switch((utils::Axis::List)axis)
{
case utils::Axis::X:
deviation.x = extent;
break;
case utils::Axis::Y:
deviation.y = extent;
break;
case utils::Axis::Z:
deviation.z = extent;
break;
}
Vec3f p,r,s;
Matrix4f head_absMat(head_absArray);
head_absMat.decompose(p,r,s);
Vec3f currentGaze = getGazeCoord();
//convert current gaze to head coord sys
currentGaze = head_absMat.inverted() * currentGaze;
//compute target vectors
Vec3f targetGaze1 = currentGaze - deviation;
Vec3f targetGaze2 = currentGaze + deviation;
//conver them to world coord sys
targetGaze1 = head_absMat * targetGaze1;
targetGaze2 = head_absMat * targetGaze2;
std::list<Gaze*> nodes;
for(int i=0; i<count; i++)
{
nodes.push_back( new Gaze(this, getGazeCoord(), (i%2 == 0)? targetGaze1 : targetGaze2, (speed >= 0)? speed : m_speed, duration) );
}
//now add the new gaze nodes to the gaze stack (in reverse order ... 'cause it's a stack)
std::list<Gaze*>::reverse_iterator riter = nodes.rbegin();
while(riter != nodes.rend())
{
if((*riter)->isValid()) m_gazeNodes.push( (*riter) );
++riter;
}
}
