/*! 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)];
}
}
}
