void QuaternionOperations::testQuaternionProd()
{
QUATERNION q_a, q_b, qTarget, qProd;
q_a.q0 = 9.999955e-01f;
q_a.q1 = -2.908411e-03f;
q_a.q2 = 7.327030e-04f;
q_a.q3 = 1.942619e-06f;
q_b.q0 = 0.000000e+00f;
q_b.q1 = -1.632653e-02f;
q_b.q2 = -6.530612e-02f;
q_b.q3 = 1.004082e+00f;
qTarget.q0 = -1.584820e-06f;
qTarget.q1 = -1.559064e-02f;
qTarget.q2 = -6.238558e-02f;
qTarget.q3 = 1.004279e+00f;
CPPUNIT_ASSERT(QuaternionProd(&q_a, &q_b, &qProd));
CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q0, qProd.q0, 1e-8f);
CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q1, qProd.q1, 1e-5f);
CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q2, qProd.q2, 1e-5f);
CPPUNIT_ASSERT_DOUBLES_EQUAL(qTarget.q3, qProd.q3, 1e-5f);
}
static void QuaternionIntegralEulerChange( time_T delta_t, const vector qdot, const vector omegadot, const quaternion q0, quaternion phi )
{
int_T j;
vector delta;
real_T delta_norm_sq;
quaternion omega0;
quaternion omega1;
quaternion omega0_omega1;
quaternion omega1_omega0;
quaternion q;
real_T delta_t3;
int_T squaring_times;
int_T k;
real_T max_elem;
/* Calculate omega from qdot */
q[0] = 2 * q0[0];
for( j=1; j<4; j++ )
{
q[j] = - 2 * q0[j];
}
QuaternionProd(q, qdot, omega0);
/* Calculate delta as Euler integral of (omega / 2) */
for( j=0; j<3; j++ )
{
delta[j] = 0.5*omega0[j+1]*delta_t;
}
/* quaternion transition matrix */
/* using repeated squaring to improve precision */
max_elem = abs(delta[0]);
for( j=1; j<3; j++ )
{
if( abs(delta[j]) > max_elem ) max_elem = abs(delta[j]);
}
squaring_times = max_elem / 0.25;
for(k = 0; k < squaring_times; k++ )
for( j=0; j<3; j++ )
{
delta[j] = delta[j] / 2;
}
delta_norm_sq = 0;
for( j=0; j<3; j++ )
{
delta_norm_sq += delta[j] * delta[j];
}
/* third order Taylor approximation */
phi[0] = 1 - 0.5 * delta_norm_sq;
for( j=0; j<3; j++ )
{
phi[j+1] = delta[j] * (1 - 1/6*delta_norm_sq);
}
for( k = 0; k < squaring_times; k++ )
{
QuaternionProd(phi, phi, q);
for( j=0; j<4; j++ )
{
phi[j] = q[j];
}
}
/* coning motion error term */
omega1[0] = 0;
for( j=0; j<3; j++ )
{
omega1[j+1] = omegadot[j];
}
QuaternionProd(omega0, omega1, omega0_omega1);
QuaternionProd(omega1, omega0, omega1_omega0);
delta_t3 = delta_t * delta_t * delta_t;
for( j=0; j<4; j++ )
{
phi[j] += 1/48*(omega0_omega1[j]-omega1_omega0[j])*delta_t3;
}
/* normalize */
/* delta_t3 = 1/(phi[0]*phi[0] + phi[1]*phi[1] + phi[2]*phi[2] + phi[3]*phi[3]);
for( j=0; j<4; j++ )
{
phi[j] = phi[j]*delta_t3;
}
*/
}
static void mdlOutputs(SimStruct *S, int_T tid)
{
PCONFIG_DATA config;
const real_T *in_xdot;
const real_T *in_x0;
InputRealPtrsType pin_params;
const boolean_T *in_reset;
real_T *out_x;
real_T *out_t;
real_T *xd;
real_T *xd_temp;
real_T *xd_temp2;
time_T initial_time;
time_T final_time;
quaternion phi;
quaternion q;
vector omegadot_temp;
const real_T *pomegadot;
int_T i;
/* Retrieve C object from the pointers vector */
config = ssGetPWork(S)[0];
xd = (real_T*) ssGetDWork(S,0);
xd_temp = (real_T*) ssGetDWork(S,1);
if( config->nq ) xd_temp2 = (real_T*) ssGetDWork(S,2);
in_xdot = ssGetInputPortRealSignal(S, config->idxin_xdot);
if( config->idxin_x0 ) in_x0 = ssGetInputPortRealSignal(S, config->idxin_x0);
if( config->idxin_params ) pin_params = ssGetInputPortRealSignalPtrs(S, config->idxin_params);
if( config->idxin_reset ) in_reset = ((InputBooleanPtrsType) ssGetInputPortSignalPtrs(S, config->idxin_reset))[0];
out_x = ssGetOutputPortRealSignal(S, 1);
if( config->idxout_time ) out_t = ssGetOutputPortRealSignal(S, config->idxout_time);
switch( intval(mxGetScalar(paramSpecificationsSource)) )
{
case 1:
initial_time = config->initial_time;
final_time = ssGetTaskTime(S,0);
break;
case 2:
initial_time = mxGetScalar(paramInitialTime);
final_time = mxGetScalar(paramFinalTime);
break;
case 3:
initial_time = *(pin_params[0]);
final_time = *(pin_params[1]);
break;
default:
ssSetErrorStatus(S,"Wrong integration algorithm selected");
return;
}
/* ssPrintf("ti=%f, tf=%f\r\n", initial_time, final_time); */
/* Reset the states */
if( ssGetIWorkValue(S, 0) || (config->idxin_reset && *in_reset) || (intval(mxGetScalar(paramSpecificationsSource)) > 1) )
{
ssSetIWorkValue(S, 0, 0);
if( intval(mxGetScalar(paramInitialConditionSource)) == 1 )
{
/* Internal initial conditions */
for( i=0; i<config->nstates; i++ )
{
xd[i] = mxGetPr(paramInitialCondition)[(mxGetNumberOfElements(paramInitialCondition) == 1 ? 0 : i)];
}
}
else
{
/* External initial conditions */
memcpy(xd, in_x0, config->nstates*sizeof(real_T));
}
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = initial_time;
}
if( final_time > initial_time )
{
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 1 )
{
/* Euler algorithm */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
QuaternionIntegralEulerChange( final_time-initial_time, &in_xdot[i], pomegadot, &xd[i], phi );
QuaternionProd(&xd[i], phi, &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd[i] += in_xdot[i] * (final_time-initial_time);
i++;
}
}
}
if( intval(mxGetScalar(paramIntegrationAlgorithm)) == 2 )
{
/* Runge-Kutta algorithm */
/* f1 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], &xd_temp[i] );
QuaternionProd(&xd_temp[i], &xd_temp[i], q);
QuaternionProd(&xd_temp[i], q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] = in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f2 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + 0.5*(final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = initial_time + 0.5*(final_time-initial_time);
/* f3 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(q, q, phi);
QuaternionProd(&xd_temp[i], phi, &xd_temp[i]);
QuaternionProd(phi, q, phi);
QuaternionProd(phi, phi, phi);
QuaternionProd(&xd[i], phi, &out_x[i]);
i += 4;
}
else
{
xd_temp[i] += 2*in_xdot[i];
out_x[i] = xd[i] + (final_time-initial_time)*in_xdot[i];
i++;
}
}
if( config->idxout_time ) out_t[0] = final_time;
/* f4 */
if( !ssCallSystemWithTid(S,0,tid) ) return;
i = 0;
while( i<config->nstates )
{
if( config->nq && (i >= config->start_idx_q) && (i < config->end_idx_q) )
{
pomegadot = ( config->use_omegadot ? &in_xdot[i] : config->omegadot_zero );
omegadot_temp[0] = pomegadot[0]*6; omegadot_temp[1] = pomegadot[1]*6; omegadot_temp[2] = pomegadot[2]*6;
QuaternionIntegralEulerChange( (final_time-initial_time)/6, &in_xdot[i], omegadot_temp, &xd[i], q );
QuaternionProd(&xd_temp[i], q, &xd_temp[i]);
QuaternionProd(&xd[i], &xd_temp[i], &xd[i]);
QuaternionNormalize(&xd[i]);
i += 4;
}
else
{
xd_temp[i] += in_xdot[i];
xd[i] += 1.0/6*(final_time-initial_time)*xd_temp[i];
i++;
}
}
}
}
else
{
if( !ssCallSystemWithTid(S,0,tid) ) return;
}
config->initial_time = final_time;
/* Update the outputs */
memcpy(out_x, xd, config->nstates*sizeof(real_T));
if( config->idxout_time ) out_t[0] = final_time;
}
