/*! performQSSSimulation(DATA* data, SOLVER_INFO* solverInfo) * * \param [ref] [data] * \param [ref] [solverInfo] * * This function performs the simulation controlled by solverInfo. */ modelica_integer prefixedName_performQSSSimulation(DATA* data, SOLVER_INFO* solverInfo) { TRACE_PUSH SIMULATION_INFO *simInfo = &(data->simulationInfo); MODEL_DATA *mData = &(data->modelData); uinteger currStepNo = 0; modelica_integer retValIntegrator = 0; modelica_integer retValue = 0; uinteger ind = 0; solverInfo->currentTime = simInfo->startTime; warningStreamPrint(LOG_STDOUT, 0, "This QSS method is under development and should not be used yet."); if (data->callback->initialAnalyticJacobianA(data)) { infoStreamPrint(LOG_STDOUT, 0, "Jacobian or sparse pattern is not generated or failed to initialize."); return UNKNOWN; } printSparseStructure(data, LOG_SOLVER); /* *********************************************************************************** */ /* Initialization */ uinteger i = 0; /* loop var */ SIMULATION_DATA *sData = (SIMULATION_DATA*)data->localData[0]; modelica_real* state = sData->realVars; modelica_real* stateDer = sData->realVars + data->modelData.nStates; const SPARSE_PATTERN* pattern = &(data->simulationInfo.analyticJacobians[data->callback->INDEX_JAC_A].sparsePattern); const uinteger ROWS = data->simulationInfo.analyticJacobians[data->callback->INDEX_JAC_A].sizeRows; const uinteger STATES = data->modelData.nStates; uinteger numDer = 0; /* number of derivatives influenced by state k */ modelica_boolean fail = 0; modelica_real* qik = NULL; /* Approximation of states */ modelica_real* xik = NULL; /* states */ modelica_real* derXik = NULL; /* Derivative of states */ modelica_real* tq = NULL; /* Time of approximation, because not all approximations are calculated at a specific time, each approx. has its own timestamp */ modelica_real* tx = NULL; /* Time of the states, because not all states are calculated at a specific time, each state has its own timestamp */ modelica_real* tqp = NULL; /* Time of the next change in state */ modelica_real* nQh = NULL; /* next value of the state */ modelica_real* dQ = NULL; /* change in quantity of every state, default = nominal*10^-4 */ /* allocate memory*/ qik = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (qik == NULL) ? 1 : ( 0 | fail); xik = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (xik == NULL) ? 1 : ( 0 | fail); derXik = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (derXik == NULL) ? 1 : ( 0 | fail); tq = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (tq == NULL) ? 1 : ( 0 | fail); tx = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (tx == NULL) ? 1 : ( 0 | fail); tqp = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (tqp == NULL) ? 1 : ( 0 | fail); nQh = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (nQh == NULL) ? 1 : ( 0 | fail); dQ = (modelica_real*)calloc(STATES, sizeof(modelica_real)); fail = (dQ == NULL) ? 1 : ( 0 | fail); if (fail) return OO_MEMORY; /* end - allocate memory */ /* further initialization of local variables */ modelica_real diffQ = 0.0, dTnextQ = 0.0, nextQ = 0.0; for (i = 0; i < STATES; i++) { dQ[i] = 0.0001 * data->modelData.realVarsData[i].attribute.nominal; tx[i] = tq[i] = simInfo->startTime; qik[i] = state[i]; xik[i] = state[i]; derXik[i] = stateDer[i]; retValue = deltaQ(data, dQ[i], i, &dTnextQ, &nextQ, &diffQ); if (OK != retValue) return retValue; tqp[i] = tq[i] + dTnextQ; nQh[i] = nextQ; } /* Transform the sparsity pattern into a data structure for an index based access. */ modelica_integer* der = (modelica_integer*)calloc(ROWS, sizeof(modelica_integer)); if (NULL==der) return OO_MEMORY; for (i = 0; i < ROWS; i++) der[i] = -1; /* how many states are involved in each derivative */ /* **** This is needed if we have QSS2 or higher **** */ /* uinteger* numStatesInDer = calloc(ROWS,sizeof(uinteger)); if (NULL==numStatesInDer) return OO_MEMORY; */ /* * dx1/dt = x2 * dx2/dt = x1 + x2 * lead to * StatesInDer[0]-{ 2 } * StatesInDer[1]-{ 1, 2 } */ /* uinteger** StatesInDer = calloc(ROWS,sizeof(uinteger*)); if (NULL==StatesInDer) return OO_MEMORY; */ /* count number of states in each derivative*/ /* for (i = 0; i < pattern->leadindex[ROWS-1]; i++) numStatesInDer[ pattern->index[i] ]++; */ /* collect memory for all stateindices */ /* for (i = 0; i < ROWS; i++) { *(StatesInDer + i) = calloc(numStatesInDer[i], sizeof(uinteger)); if (NULL==*(StatesInDer + i)) return OO_MEMORY; } retValue = getStatesInDer(pattern->index, pattern->leadindex, ROWS, STATES, StatesInDer); if (OK != retValue) return retValue; */ /* retValue = getDerWithStateK(pattern->index, pattern->leadindex, der, &numDer, 0); if (OK != retValue) return retValue; */ /* End of transformation */ #ifdef D FILE* fid=NULL; fid = fopen("log_qss.txt","w"); #endif /* *********************************************************************************** */ /***** Start main simulation loop *****/ while(solverInfo->currentTime < simInfo->stopTime) { modelica_integer success = 0; threadData->currentErrorStage = ERROR_SIMULATION; omc_alloc_interface.collect_a_little(); #if !defined(OMC_EMCC) /* try */ MMC_TRY_INTERNAL(simulationJumpBuffer) { #endif #ifdef USE_DEBUG_TRACE if (useStream[LOG_TRACE]) printf("TRACE: push loop step=%u, time=%.12g\n", currStepNo, solverInfo->currentTime); #endif #ifdef D fprintf(fid,"t = %.8f\n",solverInfo->currentTime); fprintf(fid,"%16s\t%16s\t%16s\t%16s\t%16s\t%16s\n","tx","x","dx","tq","q","tqp"); for (i = 0; i < STATES; i++) { fprintf(fid,"%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\n",tx[i],xik[i],derXik[i],tq[i],qik[i],tqp[i]); } #endif currStepNo++; ind = minStep(tqp, STATES); if (isnan(tqp[ind])) { #ifdef D fprintf(fid,"Exit caused by #QNAN!\tind=%d",ind); #endif return ISNAN; } if (isinf(tqp[ind])) { /* If all derivatives are zero, the states stay constant and only the * time propagates till stop->time. */ warningStreamPrint(LOG_STDOUT, 0, "All derivatives are zero at time %f!.\n", sData->timeValue); solverInfo->currentTime = simInfo->stopTime; sData->timeValue = solverInfo->currentTime; continue; } qik[ind] = nQh[ind]; xik[ind] = qik[ind]; state[ind] = qik[ind]; tx[ind] = tqp[ind]; tq[ind] = tqp[ind]; solverInfo->currentTime = tqp[ind]; #ifdef D fprintf(fid,"Index: %d\n\n",ind); #endif /* the state[ind] will change again in dTnextQ*/ retValue = deltaQ(data, dQ[ind], ind, &dTnextQ, &nextQ, &diffQ); if (OK != retValue) return retValue; tqp[ind] = tq[ind] + dTnextQ; nQh[ind] = nextQ; if (0 != strcmp("ia", MMC_STRINGDATA(data->simulationInfo.outputFormat))) { communicateStatus("Running", (solverInfo->currentTime-simInfo->startTime)/(simInfo->stopTime-simInfo->startTime)); } /* get the derivatives depending on state[ind] */ for (i = 0; i < ROWS; i++) der[i] = -1; retValue = getDerWithStateK(pattern->index, pattern->leadindex, der, &numDer, ind); uinteger k = 0, j = 0; for (k = 0; k < numDer; k++) { j = der[k]; if (j != ind) { xik[j] = xik[j] + derXik[j] * (solverInfo->currentTime - tx[j]); state[j] = xik[j]; tx[j] = solverInfo->currentTime; } } /* * Recalculate all equations which are affected by state[ind]. * Unfortunately all equations will be calculated up to now. And we need to evaluate * the equations as f(t,q) and not f(t,x). So all states were saved onto a local stack * and overwritten by q. After evaluating the equations the states are written back. */ for (i = 0; i < STATES; i++) { xik[i] = state[i]; /* save current state */ state[i] = qik[i]; /* overwrite current state for dx/dt = f(t,q) */ } /* update continous system */ sData->timeValue = solverInfo->currentTime; externalInputUpdate(data); data->callback->input_function(data); data->callback->functionODE(data); data->callback->functionAlgebraics(data); data->callback->output_function(data); data->callback->function_storeDelayed(data); for (i = 0; i < STATES; i++) { state[i] = xik[i]; /* restore current state */ } /* * Get derivatives affected by state[ind] and write back ALL derivatives. After that we have * states and derivatives for different times tx. */ for (k = 0; k < numDer; k++) { j = der[k]; derXik[j] = stateDer[j]; } derXik[ind] = stateDer[ind]; /* not in every case part of the above derivatives */ for (i = 0; i < STATES; i++) { stateDer[i] = derXik[i]; /* write back all derivatives */ } /* recalculate the time of next change only for the affected states */ for (k = 0; k < numDer; k++) { j = der[k]; retValue = deltaQ(data, dQ[j], j, &dTnextQ, &nextQ, &diffQ); if (OK != retValue) return retValue; tqp[j] = solverInfo->currentTime + dTnextQ; nQh[j] = nextQ; } /*sData->timeValue = solverInfo->currentTime;*/ solverInfo->laststep = solverInfo->currentTime; sim_result.emit(&sim_result, data); /* check if terminate()=true */ if (terminationTerminate) { printInfo(stdout, TermInfo); fputc('\n', stdout); infoStreamPrint(LOG_STDOUT, 0, "Simulation call terminate() at time %f\nMessage : %s", data->localData[0]->timeValue, TermMsg); simInfo->stopTime = solverInfo->currentTime; } /* terminate for some cases: * - integrator fails * - non-linear system failed to solve * - assert was called */ if (retValIntegrator) { retValue = -1 + retValIntegrator; infoStreamPrint(LOG_STDOUT, 0, "model terminate | Integrator failed. | Simulation terminated at time %g", solverInfo->currentTime); break; } else if (check_nonlinear_solutions(data, 0)) { retValue = -2; infoStreamPrint(LOG_STDOUT, 0, "model terminate | non-linear system solver failed. | Simulation terminated at time %g", solverInfo->currentTime); break; } else if (check_linear_solutions(data, 0)) { retValue = -3; infoStreamPrint(LOG_STDOUT, 0, "model terminate | linear system solver failed. | Simulation terminated at time %g", solverInfo->currentTime); break; } else if (check_mixed_solutions(data, 0)) { retValue = -4; infoStreamPrint(LOG_STDOUT, 0, "model terminate | mixed system solver failed. | Simulation terminated at time %g", solverInfo->currentTime); break; } success = 1; #if !defined(OMC_EMCC) } /* catch */ MMC_CATCH_INTERNAL(simulationJumpBuffer) #endif if (!success) { retValue = -1; infoStreamPrint(LOG_STDOUT, 0, "model terminate | Simulation terminated by an assert at time: %g", data->localData[0]->timeValue); break; } TRACE_POP /* pop loop */ } /* End of main loop */ #ifdef D fprintf(fid,"t = %.8f\n",solverInfo->currentTime); fprintf(fid,"%16s\t%16s\t%16s\t%16s\t%16s\t%16s\n","tx","x","dx","tq","q","tqp"); for (i = 0; i < STATES; i++) { fprintf(fid,"%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\t%16.8f\n",tx[i],xik[i],derXik[i],tq[i],qik[i],tqp[i]); } fclose(fid); #endif /* free memory*/ free(der); /* for (i = 0; i < ROWS; i++) free(*(StatesInDer + i)); free(StatesInDer); free(numStatesInDer); */ free(qik); free(xik); free(derXik); free(tq); free(tx); free(tqp); free(nQh); free(dQ); /* end - free memory */ TRACE_POP return retValue; }
void SensorFusion::handleMessage(const MessageBodyFrame& msg) { if (msg.Type != Message_BodyFrame) return; // Put the sensor readings into convenient local variables Vector3f angVel = msg.RotationRate; Vector3f rawAccel = msg.Acceleration; Vector3f mag = msg.MagneticField; // Set variables accessible through the class API DeltaT = msg.TimeDelta; AngV = msg.RotationRate; AngV.y *= YawMult; // Warning: If YawMult != 1, then AngV is not true angular velocity A = rawAccel; // Allow external access to uncalibrated magnetometer values RawMag = mag; // Apply the calibration parameters to raw mag if (HasMagCalibration()) { mag.x += MagCalibrationMatrix.M[0][3]; mag.y += MagCalibrationMatrix.M[1][3]; mag.z += MagCalibrationMatrix.M[2][3]; } // Provide external access to calibrated mag values // (if the mag is not calibrated, then the raw value is returned) CalMag = mag; float angVelLength = angVel.Length(); float accLength = rawAccel.Length(); // Acceleration in the world frame (Q is current HMD orientation) Vector3f accWorld = Q.Rotate(rawAccel); // Keep track of time Stage++; float currentTime = Stage * DeltaT; // Assumes uniform time spacing // Insert current sensor data into filter history FRawMag.AddElement(RawMag); FAccW.AddElement(accWorld); FAngV.AddElement(angVel); // Update orientation Q based on gyro outputs. This technique is // based on direct properties of the angular velocity vector: // Its direction is the current rotation axis, and its magnitude // is the rotation rate (rad/sec) about that axis. Our sensor // sampling rate is so fast that we need not worry about integral // approximation error (not yet, anyway). if (angVelLength > 0.0f) { Vector3f rotAxis = angVel / angVelLength; float halfRotAngle = angVelLength * DeltaT * 0.5f; float sinHRA = sin(halfRotAngle); Quatf deltaQ(rotAxis.x*sinHRA, rotAxis.y*sinHRA, rotAxis.z*sinHRA, cos(halfRotAngle)); Q = Q * deltaQ; } // The quaternion magnitude may slowly drift due to numerical error, // so it is periodically normalized. if (Stage % 5000 == 0) Q.Normalize(); // Maintain the uncorrected orientation for later use by predictive filtering QUncorrected = Q; // Perform tilt correction using the accelerometer data. This enables // drift errors in pitch and roll to be corrected. Note that yaw cannot be corrected // because the rotation axis is parallel to the gravity vector. if (EnableGravity) { // Correcting for tilt error by using accelerometer data const float gravityEpsilon = 0.4f; const float angVelEpsilon = 0.1f; // Relatively slow rotation const int tiltPeriod = 50; // Req'd time steps of stability const float maxTiltError = 0.05f; const float minTiltError = 0.01f; // This condition estimates whether the only measured acceleration is due to gravity // (the Rift is not linearly accelerating). It is often wrong, but tends to average // out well over time. if ((fabs(accLength - 9.81f) < gravityEpsilon) && (angVelLength < angVelEpsilon)) TiltCondCount++; else TiltCondCount = 0; // After stable measurements have been taken over a sufficiently long period, // estimate the amount of tilt error and calculate the tilt axis for later correction. if (TiltCondCount >= tiltPeriod) { // Update TiltErrorEstimate TiltCondCount = 0; // Use an average value to reduce noice (could alternatively use an LPF) Vector3f accWMean = FAccW.Mean(); // Project the acceleration vector into the XZ plane Vector3f xzAcc = Vector3f(accWMean.x, 0.0f, accWMean.z); // The unit normal of xzAcc will be the rotation axis for tilt correction Vector3f tiltAxis = Vector3f(xzAcc.z, 0.0f, -xzAcc.x).Normalized(); Vector3f yUp = Vector3f(0.0f, 1.0f, 0.0f); // This is the amount of rotation float tiltAngle = yUp.Angle(accWMean); // Record values if the tilt error is intolerable if (tiltAngle > maxTiltError) { TiltErrorAngle = tiltAngle; TiltErrorAxis = tiltAxis; } } // This part performs the actual tilt correction as needed if (TiltErrorAngle > minTiltError) { if ((TiltErrorAngle > 0.4f)&&(Stage < 8000)) { // Tilt completely to correct orientation Q = Quatf(TiltErrorAxis, -TiltErrorAngle) * Q; TiltErrorAngle = 0.0f; } else { //LogText("Performing tilt correction - Angle: %f Axis: %f %f %f\n", // TiltErrorAngle,TiltErrorAxis.x,TiltErrorAxis.y,TiltErrorAxis.z); //float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f; // This uses agressive correction steps while your head is moving fast float deltaTiltAngle = -Gain*TiltErrorAngle*0.005f*(5.0f*angVelLength+1.0f); // Incrementally "untilt" by a small step size Q = Quatf(TiltErrorAxis, deltaTiltAngle) * Q; TiltErrorAngle += deltaTiltAngle; } } } // Yaw drift correction based on magnetometer data. This corrects the part of the drift // that the accelerometer cannot handle. // This will only work if the magnetometer has been enabled, calibrated, and a reference // point has been set. const float maxAngVelLength = 3.0f; const int magWindow = 5; const float yawErrorMax = 0.1f; const float yawErrorMin = 0.01f; const int yawErrorCountLimit = 50; const float yawRotationStep = 0.00002f; if (angVelLength < maxAngVelLength) MagCondCount++; else MagCondCount = 0; YawCorrectionInProgress = false; if (EnableYawCorrection && MagReady && (currentTime > 2.0f) && (MagCondCount >= magWindow) && (Q.Distance(MagRefQ) < MagRefDistance)) { // Use rotational invariance to bring reference mag value into global frame Vector3f grefmag = MagRefQ.Rotate(GetCalibratedMagValue(MagRefM)); // Bring current (averaged) mag reading into global frame Vector3f gmag = Q.Rotate(GetCalibratedMagValue(FRawMag.Mean())); // Calculate the reference yaw in the global frame float gryaw = atan2(grefmag.x,grefmag.z); // Calculate the current yaw in the global frame float gyaw = atan2(gmag.x,gmag.z); //LogText("Yaw error estimate: %f\n",YawErrorAngle); // The difference between reference and current yaws is the perceived error YawErrorAngle = AngleDifference(gyaw,gryaw); // If the perceived error is large, keep count if ((fabs(YawErrorAngle) > yawErrorMax) && (!YawCorrectionActivated)) YawErrorCount++; // After enough iterations of high perceived error, start the correction process if (YawErrorCount > yawErrorCountLimit) YawCorrectionActivated = true; // If the perceived error becomes small, turn off the yaw correction if ((fabs(YawErrorAngle) < yawErrorMin) && YawCorrectionActivated) { YawCorrectionActivated = false; YawErrorCount = 0; } // Perform the actual yaw correction, due to previously detected, large yaw error if (YawCorrectionActivated) { YawCorrectionInProgress = true; int sign = (YawErrorAngle > 0.0f) ? 1 : -1; // Incrementally "unyaw" by a small step size Q = Quatf(Vector3f(0.0f,1.0f,0.0f), -yawRotationStep * sign) * Q; } } }
//------------------------------------------------------------------------------ //void CKonst(int lmax, double *cma, double *cpa, double *cza, // double *KmLm, double *KoLo, double *KpLM, // double *KmlM, double *Kolo, double *Kplm){ // int i=0; // int l, m; // for(l=0;l<=lmax;l++){ // for(m=-l;m<=l;m++){ // cma[i]=cm(l,m); // cza[i]=m; // cpa[i]=cp(l,m); // KmLm[i]=Km(l+1,m,m-1); // KoLo[i]=Ko(l+1,m,m ); // KpLM[i]=Kp(l+1,m,m+1); // KmlM[i]=Km(l ,m,m+1); // Kolo[i]=Ko(l ,m,m ); // Kplm[i]=Kp(l ,m,m-1); // i++; // } // } //} ////------------------------------------------------------------------------------ //// CALCULATION OF CONSTANTS - ONE MORE LOOP - POSITION INDEPENDENT ////------------------------------------------------------------------------------ //// L DEPENDENT //double CL[LMAX]; //double LL[LMAX]; //// CONSTANTS FOR X //double CM[LMAX]; //double CZ[LMAX]; //double CP[LMAX]; //// CONSTANTS FOR Y AND V //KmLm[LMAX]; //KoLo[LMAX]; //KpLM[LMAX]; //KmlM[LMAX]; //Kolo[LMAX]; //Kplm[LMAX]; //// SINGLE LOOP //for(int l=1;l<=lmax;l++){ // for(int m=-l; m<=l; m++){ // CL[jlm(l,m)]=l; // LL[jlm(l,m)]=2*l+1; // CM[jlm(l,m)]=cm(l,m); // CZ[jlm(l,m)]=m; // CP[jlm(l,m)]=cp(l,m); // KmLm[jlm(l,m)]=Km(l+1,m,m-1); // KoLo[jlm(l,m)]=Ko(l+1,m,m ); // KpLM[jlm(l,m)]=Kp(l+1,m,m+1); // KmlM[jlm(l,m)]=Km(l ,m,m+1); // Kolo[jlm(l,m)]=Ko(l ,m,m ); // Kplm[jlm(l,m)]=Kp(l ,m,m-1); // } //} //------------------------------------------------------------------------------ // POSITION DEPENDENT CALCULATIONS - MULTIPLES LOOPS - COMPLETE CALCULATIONS //------------------------------------------------------------------------------ void VectorSphericalWaveFunctions(double *k,double *x, double *y, double *z,int *lmax, double complex *GTE, double complex *GTM, double complex *Em, double complex *Ez, double complex *Ep, double complex *Hm, double complex *Hz, double complex *Hp ){ // printf("%d\t%E\t%E\t%E\t%E\n",*lmax+1,*k,*x,*y,*z); int l,m; int LMAX=*lmax*(*lmax+2); int LMAXE=(*lmax+1)*(*lmax+3); double cph; double sph; double rho=sqrt(*x*(*x)+*y*(*y)); double r=sqrt(rho*rho+*z*(*z)); double sth=rho/r; double cth=*z/r; if((*x==0)&&(*y==0)){ cph=1; sph=0; }else{ cph=*x/rho; sph=*y/rho; } // Spherical Bessel Funtions double JLM[*lmax+2]; // double *JLM=&JLM0[0]; gsl_sf_bessel_jl_steed_array(*lmax+1,*k*r,JLM); /* CALCULATIONS OK for(l=0;l<(*lmax+2);l++){ printf("%d\t%f\t%E\n",l,*k*r,JLM[l]); } */ // Qlm - primeiros 4 termos double Qlm[LMAXE]; Qlm[jlm(0, 0)]=1/sqrt(4*M_PI); Qlm[jlm(1, 1)]=-gammaQ(1)*sth*Qlm[jlm(0,0)]; // Q11 Qlm[jlm(1, 0)]=sqrt(3.0)*cth*Qlm[jlm(0,0)]; // Q10 Qlm[jlm(1,-1)]=-Qlm[jlm(1,1)]; // Q11*(-1) // Complex Exponencial for m=-1,0,1 double complex Eim[2*(*lmax)+3]; Eim[*lmax-1]=(cph-I*sph); Eim[*lmax ]=1+I*0; Eim[*lmax+1]=(cph+I*sph); // Ylm - primeiros 4 termos double complex Ylm[LMAXE]; Ylm[jlm(0, 0)]=Qlm[jlm(0, 0)]; Ylm[jlm(1,-1)]=Qlm[jlm(1,-1)]*Eim[*lmax-1]; Ylm[jlm(1, 0)]=Qlm[jlm(1, 0)]; Ylm[jlm(1, 1)]=Qlm[jlm(1, 1)]*Eim[*lmax+1]; /* OK jl, Qlm, Ylm for(l=0;l<2;l++){ for(m=-l;m<=l;m++){ printf("%d\t%d\t%d\t%f\t%f\t%f+%fi\n",l,m,jlm(l,m),JLM[jlm(l,m)],Qlm[jlm(l,m)],creal(Ylm[jlm(l,m)]),cimag(Ylm[jlm(l,m)])); } } printf("======================================================================\n"); */ // VECTOR SPHERICAL HARMONICS double complex XM; //[LMAX]; double complex XZ; //[LMAX]; double complex XP; //[LMAX]; double complex YM; //[LMAX]; double complex YZ; //[LMAX]; double complex YP; //[LMAX]; double complex VM; //[LMAX]; double complex VZ; //[LMAX]; double complex VP; //[LMAX]; // HANSEN MULTIPOLES double complex MM; //[LMAX]; double complex MZ; //[LMAX]; double complex MP; //[LMAX]; double complex NM; //[LMAX]; double complex NZ; //[LMAX]; double complex NP; //[LMAX]; // OTHERS double kl; // MAIN LOOP for(l=1;l<=(*lmax);l++){ //------------------------------------------------------------------------------ //Qlm extremos positivos um passo a frente Qlm[jlm(l+1, l+1)]=-gammaQ(l+1)*sth*Qlm[jlm(l,l)]; Qlm[jlm(l+1, l )]= deltaQ(l+1)*cth*Qlm[jlm(l,l)]; //Qlm extremos negativos um passo a frente Qlm[jlm(l+1,-l-1)]=pow(-1,l+1)*Qlm[jlm(l+1, l+1)]; Qlm[jlm(l+1,-l )]=pow(-1,l )*Qlm[jlm(l+1, l )]; // Exponenciais um passo a frente Eim[*lmax+l+1]=Eim[*lmax+l]*(cph+I*sph); Eim[*lmax-l-1]=Eim[*lmax-l]*(cph-I*sph); // Harmonicos esfericos extremos um passo a frente Ylm[jlm(l+1, l+1)]=Qlm[jlm(l+1, l+1)]*Eim[*lmax+l+1]; Ylm[jlm(l+1, l )]=Qlm[jlm(l+1, l )]*Eim[*lmax+l ]; Ylm[jlm(l+1,-l-1)]=Qlm[jlm(l+1,-l-1)]*Eim[*lmax-l-1]; Ylm[jlm(l+1,-l )]=Qlm[jlm(l+1,-l )]*Eim[*lmax-l ]; // others kl=1/(sqrt(l*(l+1))); for(m=0; m<l; m++){ // Demais valores de Qlm e Ylm Qlm[jlm(l+1, m)]=alfaQ(l+1,m)*cth*Qlm[jlm(l,m)]-betaQ(l+1,m)*Qlm[jlm(l-1,m)]; Qlm[jlm(l+1,-m)]=pow(-1,m)*Qlm[jlm(l+1, m)]; Ylm[jlm(l+1, m)]=Qlm[jlm(l+1, m)]*Eim[*lmax+m]; Ylm[jlm(l+1,-m)]=Qlm[jlm(l+1,-m)]*Eim[*lmax-m]; } //------------------------------------------------------------------------------ // for(m=-(l+1); m<=(l+1); m++){ // Ylm[jlm(l+1,m)]=Qlm[jlm(l+1,m)]*Eim[*lmax+m]; // } // for(m=-l; m<=l; m++){ // printf("%d\t%d\t%d\t%f\t%f+%fi\t%f+%fi\n",l,m,jlm(l,m)+1,Qlm[jlm(l,m)],creal(Eim[*lmax+m]),cimag(Eim[*lmax+m]),creal(Ylm[jlm(l,m)]),cimag(Ylm[jlm(l,m)])); // } //------------------------------------------------------------------------------ for(int m=-l; m<=l; m++){ XM=kl*cm(l,m)*Ylm[jlm(l,m-1)]/sqrt(2); XZ=kl*m*Ylm[jlm(l,m )]; XP=kl*cp(l,m)*Ylm[jlm(l,m+1)]/sqrt(2); // printf("--------X--------------------------------\n"); printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(XM),cimag(XM)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(XZ),cimag(XZ)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(XP),cimag(XP)); YM=(-Kp(l,m,m-1)*Ylm[jlm(l,m-1)]+Km(l+1,m,m-1)*Ylm[jlm(l+1,m-1)])/sqrt(2); YZ= Ko(l,m,m )*Ylm[jlm(l,m )]+Ko(l+1,m,m )*Ylm[jlm(l+1,m )]; YP=( Km(l,m,m+1)*Ylm[jlm(l,m+1)]-Kp(l+1,m,m+1)*Ylm[jlm(l+1,m+1)])/sqrt(2); // printf("--------Y--------------------------------\n"); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(YM),cimag(YM)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(YZ),cimag(YZ)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(YP),cimag(YP)); VM=kl*(-(l+1)*Kp(l,m,m-1)*Ylm[jlm(l,m-1)]-l*Km(l+1,m,m-1)*Ylm[jlm(l+1,m-1)])/sqrt(2); VZ=kl*( (l+1)*Ko(l,m,m )*Ylm[jlm(l,m )]-l*Ko(l+1,m,m )*Ylm[jlm(l+1,m )]); VP=kl*( (l+1)*Km(l,m,m+1)*Ylm[jlm(l,m+1)]+l*Kp(l+1,m,m+1)*Ylm[jlm(l+1,m+1)])/sqrt(2); // printf("--------V--------------------------------\n"); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(VM),cimag(VM)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(VZ),cimag(VZ)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(VP),cimag(VP)); // CALCULATION OF HANSEM MULTIPOLES MM=JLM[l]*XM; MZ=JLM[l]*XZ; MP=JLM[l]*XP; // printf("--------M--------------------------------\n"); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(MM),cimag(MM)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(MZ),cimag(MZ)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(MP),cimag(MP)); NM=((l+1)*JLM[l-1]-l*JLM[l+1])*VM/(2*l+1)+sqrt(l*(l+1))*JLM[l]*YM; NZ=((l+1)*JLM[l-1]-l*JLM[l+1])*VZ/(2*l+1)+sqrt(l*(l+1))*JLM[l]*YZ; NP=((l+1)*JLM[l-1]-l*JLM[l+1])*VP/(2*l+1)+sqrt(l*(l+1))*JLM[l]*YP; // printf("--------N--------------------------------\n"); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(NM),cimag(NM)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(NZ),cimag(NZ)); // printf("%d\t%d\t%d\t%f+%fi\n",l,m,jlm(l,m),creal(NP),cimag(NP)); // CALCULATION OF THE ELECTROMAGNETIC FIELDS *Em=*Em+MM*GTE[jlm(l,m)]-NM*GTM[jlm(l,m)]; *Ez=*Ez+MZ*GTE[jlm(l,m)]-NZ*GTM[jlm(l,m)]; *Ep=*Ep+MP*GTE[jlm(l,m)]-NP*GTM[jlm(l,m)]; *Hm=*Hm+MM*GTM[jlm(l,m)]+NM*GTE[jlm(l,m)]; *Hz=*Hz+MZ*GTM[jlm(l,m)]+NZ*GTE[jlm(l,m)]; *Hp=*Hp+MP*GTM[jlm(l,m)]+NP*GTE[jlm(l,m)]; } } }