const Quaternion QuaternionManipulator::qFromAngleAxis(float theta, Vector axis)
{
Vector axisn = vNormalize(axis);
Quaternion q;
float a = theta * (float)DEGREES_TO_RADIANS;
q.t = cos(a / 2.0f);
float s = sin(a / 2.0f);
q.x = axisn.x * s;
q.y = axisn.y * s;
q.z = axisn.z * s;
qClean(q);
return qNormalize(q);
//return q;
}
// -----------------------------------------------------------------------------
int
iDcmLoop (void) {
static uint16_t usLoopCount;
// Indicate that the DCM is ready when > 5s.
if (++usLoopCount > (CFG_DCM_READY_DELAY / DCM_DT)) {
xState.bReady = 1;
}
// Read from the sensors
vReadSensors();
if (xState.iError >= 0) {
// Predict next state of DCM
vDcmPredict();
// Normalize the DCM
vNormalize();
// Correct the gyro drift
vDcmCorrectDrift();
// Convert the DCM to Euler angle
vCalculateEuler();
// Convert the yaw angle to 0 - 2PI
CONVERT_0_2PI(xAttitudeTemp.fAtt[YAW]);
// Calculate the actual rate
vVector3fAdd (xAttitudeTemp.fRate, fGyroVector, fRateCorrectionI);
// Copy the attitude to the private global variable.
// We don't want to switch the context during this process
ATOMIC_BLOCK(ATOMIC_RESTORESTATE) {
xAttitude = xAttitudeTemp;
}
if (xState.bReady == 0)
xState.iError = DCM_NOT_READY;
}
void InitBeam(void)
// initialize beam; produce description string
{
double w0; // beam width
/* TO ADD NEW BEAM
* Add here all intermediate variables, which are used only inside this function.
*/
// initialization of global option index for error messages
opt=opt_beam;
// beam initialization
switch (beamtype) {
case B_PLANE:
if (IFROOT) strcpy(beam_descr,"plane wave");
beam_asym=false;
if (surface) {
if (prop_0[2]==0) PrintError("Ambiguous setting of beam propagating along the surface. Please specify "
"the incident direction to have (arbitrary) small positive or negative z-component");
if (msubInf && prop_0[2]>0) PrintError("Perfectly reflecting surface ('-surf ... inf') is incompatible "
"with incident direction from below (including the default one)");
// Here we set ki,kt,ktVec and propagation directions prIncRefl,prIncTran
if (prop_0[2]>0) { // beam comes from the substrate (below)
// here msub should always be defined
inc_scale=1/creal(msub);
ki=msub*prop_0[2];
/* Special case for msub near 1 to remove discontinuities for near-grazing incidence. The details
* are discussed in CalcFieldSurf() in crosssec.c.
*/
if (cabs(msub-1)<ROUND_ERR && fabs(ki)<SQRT_RND_ERR) kt=ki;
else kt=cSqrtCut(1 - msub*msub*(prop_0[0]*prop_0[0]+prop_0[1]*prop_0[1]));
// determine propagation direction and full wavevector of wave transmitted into substrate
ktVec[0]=msub*prop_0[0];
ktVec[1]=msub*prop_0[1];
ktVec[2]=kt;
}
else if (prop_0[2]<0) { // beam comes from above the substrate
inc_scale=1;
vRefl(prop_0,prIncRefl);
ki=-prop_0[2];
if (!msubInf) {
// same special case as above
if (cabs(msub-1)<ROUND_ERR && fabs(ki)<SQRT_RND_ERR) kt=ki;
else kt=cSqrtCut(msub*msub - (prop_0[0]*prop_0[0]+prop_0[1]*prop_0[1]));
// determine propagation direction of wave transmitted into substrate
ktVec[0]=prop_0[0];
ktVec[1]=prop_0[1];
ktVec[2]=-kt;
}
}
else LogError(ONE_POS,"Ambiguous setting of beam propagating along the surface. Please specify the"
"incident direction to have (arbitrary) small positive or negative z-component");
vRefl(prop_0,prIncRefl);
if (!msubInf) {
vReal(ktVec,prIncTran);
vNormalize(prIncTran);
}
}
return;
case B_DIPOLE:
vCopy(beam_pars,beam_center_0);
if (surface) {
if (beam_center_0[2]<=-hsub)
PrintErrorHelp("External dipole should be placed strictly above the surface");
inc_scale=1; // but scaling of Mueller matrix is weird anyway
}
// in weird scenarios the dipole can be positioned exactly at the origin; reused code from Gaussian beams
beam_asym=(beam_center_0[0]!=0 || beam_center_0[1]!=0 || beam_center_0[2]!=0);
if (!beam_asym) vInit(beam_center);
/* definition of p0 is important for scaling of many scattering quantities (that are normalized to incident
* irradiance). Alternative definition is p0=1, but then the results will scale with unit of length
* (breaking scale invariance)
*/
p0=1/(WaveNum*WaveNum*WaveNum);
if (IFROOT) sprintf(beam_descr,"point dipole at "GFORMDEF3V,COMP3V(beam_center_0));
return;
case B_LMINUS:
case B_DAVIS3:
case B_BARTON5:
if (surface) PrintError("Currently, Gaussian incident beam is not supported for '-surf'");
// initialize parameters
w0=beam_pars[0];
TestPositive(w0,"beam width");
vCopy(beam_pars+1,beam_center_0);
beam_asym=(beam_Npars==4 && (beam_center_0[0]!=0 || beam_center_0[1]!=0 || beam_center_0[2]!=0));
if (!beam_asym) vInit(beam_center);
s=1/(WaveNum*w0);
s2=s*s;
scale_x=1/w0;
scale_z=s*scale_x; // 1/(k*w0^2)
// beam info
if (IFROOT) {
strcpy(beam_descr,"Gaussian beam (");
switch (beamtype) {
case B_LMINUS:
strcat(beam_descr,"L- approximation)\n");
break;
case B_DAVIS3:
strcat(beam_descr,"3rd order approximation, by Davis)\n");
break;
case B_BARTON5:
strcat(beam_descr,"5th order approximation, by Barton)\n");
break;
default: break;
}
sprintf(beam_descr+strlen(beam_descr),"\tWidth="GFORMDEF" (confinement factor s="GFORMDEF")\n"
"\tCenter position: "GFORMDEF3V,w0,s,COMP3V(beam_center_0));
}
return;
case B_READ:
// the safest is to assume cancellation of all symmetries
symX=symY=symZ=symR=false;
if (surface) inc_scale=1; // since we can't know it, we assume the default case
if (IFROOT) {
if (beam_Npars==1) sprintf(beam_descr,"specified by file '%s'",beam_fnameY);
else sprintf(beam_descr,"specified by files '%s' and '%s'",beam_fnameY,beam_fnameX);
}
// we do not define beam_asym here, because beam_center is not defined anyway
return;
}
LogError(ONE_POS,"Unknown type of incident beam (%d)",(int)beamtype);
/* TO ADD NEW BEAM
* add a case above. Identifier ('B_...') should be defined inside 'enum beam' in const.h. The case should
* 1) save all the input parameters from array 'beam_pars' to local variables (defined in the beginning of this
* source files)
* 2) test all input parameters (for that you're encouraged to use functions from param.h since they would
* automatically produce informative output in case of error).
* 3) the symmetry breaking due to prop or beam_center is taken care of in VariablesInterconnect() in param.c.
* But if there are other reasons why beam would break any symmetry, corresponding variable should be set to
* false here. Do not set any of them to true, as they can be set to false by other factors.
* symX, symY, symZ - symmetries of reflection over planes YZ, XZ, XY respectively.
* symR - symmetry of rotation for 90 degrees over the Z axis
* 4) initialize the following:
* beam_descr - descriptive string, which will appear in log file.
* beam_asym - whether beam center does not coincide with the reference frame origin. If it is set to true, then
* set also beam_center_0 - 3D radius-vector of beam center in the laboratory reference frame (it
* will be then automatically transformed to particle reference frame, if required).
* 5) Consider the case of surface (substrate near the particle). If the new beam type is incompatible with it, add
* an explicit exception, like "if (surface) PrintErrorHelp(...);". Otherwise, you also need to define inc_scale.
* All other auxiliary variables, which are used in beam generation (GenerateB(), see below), should be defined in
* the beginning of this file. If you need temporary local variables (which are used only in this part of the code),
* define them in the beginning of this function.
*/
}
