void CVehicleMovementAerodynamic::UpdateWing(SWing *_pWing,float _fAngle,float _fDeltaTime)
{
Matrix34 matWing = m_pEntity->GetWorldTM() * Matrix33::CreateRotationXYZ(Ang3(DEG2RAD(_pWing->fAngleX),
DEG2RAD(_pWing->fAngleY),
DEG2RAD(_pWing->fAngleZ))).GetInverted();
Vec3 vRight = matWing.GetColumn(0);
Vec3 vLook = matWing.GetColumn(1);
Vec3 vUp = matWing.GetColumn(2);
pe_status_dynamics StatusDynamics;
GetPhysics()->GetStatus(&StatusDynamics);
// v(relativepoint) = v + w^(relativepoint-center)
Vec3 vVelocity = StatusDynamics.v + StatusDynamics.w.Cross(m_pEntity->GetWorldTM().TransformVector(_pWing->vPos));
Vec3 vVelocityNormalized = vVelocity.GetNormalizedSafe(vLook);
// TODO:
float fAngleOfAttack = RAD2DEG(asin(vRight.Dot(vVelocityNormalized.Cross(vLook))));
DumpText("AoA=%f",fAngleOfAttack);
fAngleOfAttack += _fAngle;
float Cl = GetCoefficient(_pWing->pLiftPointsMap,fAngleOfAttack) * _pWing->fCl;
float Cd = GetCoefficient(_pWing->pDragPointsMap,fAngleOfAttack) * _pWing->fCd;
Vec3 vVelocityNormal = vRight.Cross(vVelocityNormalized).GetNormalized();
float fVelocitySquared = vVelocity.len2();
const float c_fDynamicPressure = 1.293f;
float fLift = 0.5f * c_fDynamicPressure * _pWing->fSurface * Cl * fVelocitySquared * _fDeltaTime;
float fDrag = 0.5f * c_fDynamicPressure * _pWing->fSurface * Cd * fVelocitySquared * _fDeltaTime;
Vec3 vLiftImpulse = +fLift * vVelocityNormal;
Vec3 vDragImpulse = -fDrag * vVelocityNormalized;
AddForce(&vLiftImpulse,&_pWing->vPos,ColorF(0,1,0,1));
AddForce(&vDragImpulse,&_pWing->vPos,ColorF(1,0,1,1));
}
/*
Integral tag dispatch, used for requirement 8.
*/
template <typename C> C Polynomial<C>::CalculateIntegralDispatch(const C a, const C b, std::false_type) const
{
auto p = this;
/*
Lambda expression used to calculate one part of an integral.
Solves requirement 9.
In addition, I'm using auto here to deduce types.
This solves requirement 3.
Furthermore, it caches its results to minimize calculations. This is thread-safe.
Solves requirement 6.
*/
auto IntegralPart = [p](const auto n){
auto res = 0.;
if (p->pImpl->integralData.count(n) > 0) //key exists, set only once
{
std::cout << "Retrieving: " << n << " from integral cache..." << std::endl;
res = p->pImpl->integralData.find(n)->second;
}
else //key doesn't exist, Calculate...
{
//Lock cache, as we want to alter it
std::lock_guard<std::mutex> lock(p->integralGuard);
//Calculate integral
for (auto i = 0; i <= p->GetHighestCoefficient(); i++)
{
res += p->GetCoefficient(i) * std::pow(n, i + 1) / (i + 1);
}
//Store in cache
p->pImpl->integralData.insert({ n, res });
}
return res;
};
/*
Run the integral part lambda concurrently.
Solves requirement 10.
*/
auto partB = std::async(IntegralPart, b);
auto partA = std::async(IntegralPart, a);
//Get results from tasks
return partB.get() - partA.get();
}
void calculate_mfcc(double spectrum[], double *mfcc_result[], unsigned int i, unsigned int samples_per_frame, FILE *fp)
{
// Determine which MFCC coefficient to compute
unsigned int coeff;
// File containing MFCC coefficents for each frame
//FILE *fp1 = fopen(mfcc_txt,"w+");
// Compute the first 13 coefficients
for (coeff = 1; coeff < 14; coeff++)
{
mfcc_result[i][coeff] = GetCoefficient(spectrum, 44100, 48, samples_per_frame, coeff);
fprintf(fp,"%i:%lf ", coeff, mfcc_result[i][coeff]);
//fprintf(fp1,"%i %lf\n ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp,"\n");
//fclose(fp1);
}
int main(int argc, char *argv[])
{
FILE *fp = stdin, *fpc;
char *coef = NULL;
double *x = NULL, *dx = NULL, **dw_coef = NULL, *y = NULL;
int i, j, l, d, t, tj, ispipe, fsize, leng = LENG, total = T;
int dw_num = 1, **dw_width = NULL, dw_calccoef = -1, dw_coeflen = 1,
dw_leng = 1;
char **dw_fn = (char **) calloc(sizeof(char *), argc);
int non_magic_num, win_size_forward[2], win_size_backward[2];
float_list *top, *cur, *prev, *tmpf;
if ((cmnd = strrchr(argv[0], '/')) == NULL)
cmnd = argv[0];
else
cmnd++;
while (--argc) {
if (**++argv == '-') {
switch (*(*argv + 1)) {
case 'd':
if (dw_calccoef == 1 || dw_calccoef == 2) {
fprintf(stderr,
"%s : Options '-r' and '-d' should not be defined simultaneously!\n",
cmnd);
return (1);
}
dw_calccoef = 0;
if (isfloat(*++argv)) {
dw_coeflen = 0;
for (i = 0; (i < argc - 1) && isfloat(argv[i]); i++) {
dw_coeflen += strlen(argv[i]) + 1;
}
dw_coeflen += 1;
coef = dw_fn[dw_num] = getmem(dw_coeflen, sizeof(char));
for (j = 0; j < i; j++) {
sprintf(coef, " %s", *argv);
coef += strlen(*argv) + 1;
if (j < i - 1) {
argv++;
argc--;
}
}
} else {
dw_fn[dw_num] = *argv;
}
dw_num++;
--argc;
break;
case 'r':
if (dw_calccoef == 0 || dw_calccoef == 2) {
fprintf(stderr,
"%s : Options '-r' and '-d' should not be defined simultaneously!\n",
cmnd);
return (1);
}
dw_calccoef = 1;
dw_coeflen = atoi(*++argv);
--argc;
if ((dw_coeflen != 1) && (dw_coeflen != 2)) {
fprintf(stderr,
"%s : Number of delta parameter should be 1 or 2!\n",
cmnd);
return (1);
}
if (argc <= 1) {
fprintf(stderr,
"%s : Window size for delta parameter required!\n",
cmnd);
return (1);
}
dw_fn[dw_num] = *++argv;
dw_num++;
--argc;
if (dw_coeflen == 2) {
if (argc <= 1) {
fprintf(stderr,
"%s : Window size for delta-delta parameter required!\n",
cmnd);
return (1);
}
dw_fn[dw_num] = *++argv;
dw_num++;
--argc;
}
break;
case 'm':
leng = atoi(*++argv) + 1;
--argc;
break;
case 'l':
leng = atoi(*++argv);
--argc;
break;
case 'R':
if (dw_calccoef == 0 || dw_calccoef == 1) {
fprintf(stderr,
"%s : Options '-r', '-d' and '-R' should not be defined simultaneously!\n",
cmnd);
return (1);
}
dw_calccoef = 2;
dw_num = atoi(*++argv) + 1;
--argc;
if ((dw_num != 2) && (dw_num != 3)) {
fprintf(stderr,
"%s : Number of delta parameter should be 1 or 2!\n",
cmnd);
return (1);
}
if (argc <= 1) {
fprintf(stderr,
"%s : Window size for delta-delta parameter required!\n",
cmnd);
return (1);
}
sscanf(*++argv, "%d", &win_size_forward[0]);
--argc;
sscanf(*++argv, "%d", &win_size_backward[0]);
--argc;
if (dw_num > 2) {
sscanf(*++argv, "%d", &win_size_forward[1]);
--argc;
sscanf(*++argv, "%d", &win_size_backward[1]);
--argc;
}
break;
case 'M':
sscanf(*++argv, "%lf", &magic);
MAGIC = TR;
--argc;
break;
case 'h':
usage(0);
default:
fprintf(stderr, "%s : Invalid option '%c'!\n", cmnd, *(*argv + 1));
usage(1);
}
} else
fp = getfp(*argv, "rb");
}
/* parse window files */
/* memory allocation */
if ((dw_width = (int **) calloc(dw_num, sizeof(int *))) == NULL) {
fprintf(stderr, "%s : Cannot allocate memory!\n", cmnd);
exit(1);
}
for (i = 0; i < dw_num; i++)
if ((dw_width[i] = (int *) calloc(2, sizeof(int))) == NULL) {
fprintf(stderr, "%s : Cannot allocate memory!\n", cmnd);
exit(1);
}
if ((dw_coef = (double **) calloc(dw_num, sizeof(double *))) == NULL) {
fprintf(stderr, "%s : Cannot allocate memory!\n", cmnd);
exit(1);
}
/* window for static parameter */
dw_width[0][0] = dw_width[0][1] = 0;
dw_coef[0] = dgetmem(1);
dw_coef[0][0] = 1;
/* set delta coefficients */
if (dw_calccoef == 0) {
for (i = 1; i < dw_num; i++) {
if (dw_fn[i][0] == ' ') {
fsize = str2darray(dw_fn[i], &(dw_coef[i]));
} else {
/* read from file */
fpc = getfp(dw_fn[i], "rb");
/* check the number of coefficients */
fseek(fpc, 0L, 2);
fsize = ftell(fpc) / sizeof(real);
fseek(fpc, 0L, 0);
/* read coefficients */
dw_coef[i] = dgetmem(fsize);
freadf(dw_coef[i], sizeof(**(dw_coef)), fsize, fpc);
}
/* set pointer */
dw_leng = fsize / 2;
dw_coef[i] += dw_leng;
dw_width[i][0] = -dw_leng;
dw_width[i][1] = dw_leng;
if (fsize % 2 == 0)
dw_width[i][1]--;
}
} else if (dw_calccoef == 1) {
int a0, a1, a2;
for (i = 1; i < dw_num; i++) {
dw_leng = atoi(dw_fn[i]);
if (dw_leng < 1) {
fprintf(stderr,
"%s : Width for regression coefficient shuould be more than 1!\n",
cmnd);
exit(1);
}
dw_width[i][0] = -dw_leng;
dw_width[i][1] = dw_leng;
dw_coef[i] = dgetmem(dw_leng * 2 + 1);
dw_coef[i] += dw_leng;
}
dw_leng = atoi(dw_fn[1]);
for (a1 = 0, j = -dw_leng; j <= dw_leng; a1 += j * j, j++);
for (j = -dw_leng; j <= dw_leng; j++)
dw_coef[1][j] = (double) j / (double) a1;
if (dw_num > 2) {
dw_leng = atoi(dw_fn[2]);
for (a0 = a1 = a2 = 0, j = -dw_leng; j <= dw_leng;
a0++, a1 += j * j, a2 += j * j * j * j, j++);
for (j = -dw_leng; j <= dw_leng; j++)
dw_coef[2][j] =
2 * ((double) (a0 * j * j - a1)) /
((double) (a2 * a0 - a1 * a1));
}
}
/* -- Count number of input vectors and read -- */
x = dgetmem(leng);
top = prev = (float_list *) malloc(sizeof(float_list));
top->f = (float *) malloc(sizeof(float) * leng);
total = 0;
prev->next = NULL;
while (freadf(x, sizeof(*x), leng, fp) == leng) {
cur = (float_list *) malloc(sizeof(float_list));
cur->f = (float *) malloc(sizeof(float) * leng);
for (i = 0; i < leng; i++) {
cur->f[i] = (float) x[i];
}
total++;
prev->next = cur;
cur->next = NULL;
prev = cur;
}
free(x);
x = dgetmem(leng * total);
dx = dgetmem(dw_num * leng * total);
fillz(dx, sizeof(*x), dw_num * leng * total);
for (i = 0, tmpf = top->next; tmpf != NULL; i++, tmpf = tmpf->next) {
for (j = 0; j < leng; j++) {
x[i * leng + j] = tmpf->f[j];
}
}
if (dw_calccoef == 0 || dw_calccoef == 1) {
/* calculate delta and delta-delta */
for (t = 0; t < total; t++) {
for (d = 0; d < dw_num; d++) {
for (j = dw_width[d][0]; j <= dw_width[d][1]; j++) {
tj = t + j;
if (tj < 0)
tj = 0;
if (!(tj < total))
tj = total - 1;
for (l = 0; l < leng; l++)
dx[dw_num * leng * t + leng * d + l] +=
dw_coef[d][j] * x[leng * tj + l];
}
}
}
/* output static, delta, delta-delta */
fwritef(dx, sizeof(*dx), dw_num * total * leng, stdout);
} else if (dw_calccoef == 2) {
int *position = (int *) malloc(sizeof(int) * total);
/* skip magic number */
if (MAGIC == TR) {
for (t = 0, non_magic_num = 0; t < total; t++) {
for (l = 0; l < leng; l++) {
if (x[leng * t + l] == magic) {
break;
}
}
if (l == leng) {
/* remember position of non-magic number */
position[non_magic_num] = t;
non_magic_num++;
}
}
} else {
for (t = 0; t < total; t++) {
position[t] = t;
}
non_magic_num = total;
}
/* calculate delta and delta-delta */
GetCoefficient(x, dx, dw_num, position, total, non_magic_num, leng,
win_size_forward, win_size_backward);
/* output static, delta, delta-delta */
fwritef(dx, sizeof(*dx), dw_num * total * leng, stdout);
}
return (0);
}
float CGaussianCPD::GetCoefficientVec( const intVector& parentCombination )
{
return GetCoefficient( &parentCombination.front() );
}
void dft(short int *x[], unsigned int samples_per_frame, const char* output)
{
// time and frequency domain data arrays
unsigned int n, k, i; // indices for time and frequency domains
//double x[N]; // discrete-time signal, x
//float Xre[num_frames][N], Xim[num_frames][N];
//float P[num_frames][N]; // power spectrum of x
unsigned int coeff;
double *mfcc_result[FRAMES_25ms];
double *Xre[FRAMES_25ms];
double *Xim[FRAMES_25ms];
double *P[FRAMES_25ms];
char *mfcc_coeff[FRAMES_25ms];
char fname[10][50];
char extension[10][10];
for(i=0;i<10;i++)
{
sprintf(extension[i],"%d.txt",i+1);
}
for(i=0;i<10;i++)
{
strcpy(fname[i],output);
strcat(fname[i], extension[i]);
}
FILE *fp1 = fopen(fname[0],"w+");
FILE *fp2 = fopen(fname[1],"w+");
FILE *fp3 = fopen(fname[2],"w+");
FILE *fp4 = fopen(fname[3],"w+");
FILE *fp5 = fopen(fname[4],"w+");
FILE *fp6 = fopen(fname[5],"w+");
FILE *fp7 = fopen(fname[6],"w+");
FILE *fp8 = fopen(fname[7],"w+");
FILE *fp9 = fopen(fname[8],"w+");
FILE *fp10 = fopen(fname[9],"w+");
for (i = 0; i < FRAMES_25ms; i++)
{
mfcc_result[i] = (double *)malloc(13 * sizeof(double)); // MFCC for each frame
Xre[i] = (double *)malloc(samples_per_frame * sizeof(double)); // DFT of x (real part)
Xim[i] = (double *)malloc(samples_per_frame * sizeof(double)); // DFT of x (imaginary part)
P[i] = (double *)malloc(samples_per_frame * sizeof(double)); // Power Spectrum
mfcc_coeff[i] = (char *)malloc(100 * sizeof(char)); // Names of txt files containing mfcc co-efficients for each frame
for (k = samples_per_frame ; k-- ; )
{
// Real part of X[k]
Xre[i][k] = 0;
// Imaginary part of X[k]
Xim[i][k] = 0;
for (n = samples_per_frame ; n-- ; )
{
Xre[i][k] += (double)(x[i][n] * cos(n * k * PI * 2 / samples_per_frame));
Xim[i][k] -= (double)(x[i][n] * sin(n * k * PI * 2 / samples_per_frame));
}
// Power at kth frequency bin
P[i][k] = Xre[i][k]*Xre[i][k] + Xim[i][k]*Xim[i][k];
}
// Compute the first 13 coefficients
for (coeff = 1; coeff < 14; coeff++)
{
mfcc_result[i][coeff] = GetCoefficient(P[i], 44100, 48, samples_per_frame, coeff);
}
fprintf(fp1, "1 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp1,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp1,"\n");
fprintf(fp2, "2 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp2,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp2,"\n");
fprintf(fp3, "3 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp3,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp3,"\n");
fprintf(fp4, "4 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp4,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp4,"\n");
fprintf(fp5, "5 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp5,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp5,"\n");
fprintf(fp6, "6 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp6,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp6,"\n");
fprintf(fp7, "7 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp7,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp7,"\n");
fprintf(fp8, "8 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp8,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp8,"\n");
fprintf(fp9, "9 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp9,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp9,"\n");
fprintf(fp10, "10 ");
for (coeff = 1; coeff < 14; coeff++)
{
fprintf(fp10,"%i:%lf ", coeff, mfcc_result[i][coeff]);
}
fprintf(fp10,"\n");
}
}
