void CLocalFrame::euler2iang (const CVector &eulerAngle, const SPosition &pos, CVector &tmp_iang) {
// TODO - Transform orientation to local coordinates, in iang
// this is a rotation
//
// 1) transform local position to inertial position
double rad_pos_x = arcseconds_lat2radians (pos.lat); // rad
double rad_pos_y = arcseconds_lat2radians (pos.lon); // rad
double tmp_pos_z = EQUATORIAL_RADIUS_FEET + pos.alt; // ft
double inert_pos_x = tmp_pos_z * cos (rad_pos_x) * cos (rad_pos_y); // ft
double inert_pos_y = tmp_pos_z * cos (rad_pos_x) * sin (rad_pos_y); // ft
double inert_pos_z = tmp_pos_z * sin (rad_pos_x); // ft
//
// 2) transform local orientation to inertial orientation
double rot_angle_x = HALF_PI - rad_pos_x;
double rot_angle_y = HALF_PI + rad_pos_y;
double sin_half_pos_x = sin (rot_angle_x / 2);
double sin_half_pos_y = sin (rot_angle_y / 2);
double cos_half_pos_x = cos (rot_angle_x / 2);
double cos_half_pos_y = cos (rot_angle_y / 2);
//
double sin_half_rot_p = sin (eulerAngle.x / 2);
double sin_half_rot_r = sin (eulerAngle.y / 2); // RH
double sin_half_rot_h = sin (eulerAngle.z / 2); // RH
//double sin_half_rot_r = sin (eulerAngle.z / 2); // LH
//double sin_half_rot_h = sin (eulerAngle.y / 2); // LH
double cos_half_rot_p = cos (eulerAngle.x / 2);
double cos_half_rot_r = cos (eulerAngle.y / 2); // RH
double cos_half_rot_h = cos (eulerAngle.z / 2); // RH
//double cos_half_rot_r = cos (eulerAngle.z / 2); // LH
//double cos_half_rot_h = cos (eulerAngle.y / 2); // LH
#ifdef _DEBUG_FRAME_MANAGER
{ FILE *fp_debug;
if(!(fp_debug = fopen("__DDEBUG_AERO_frames.txt", "a")) == NULL)
{
fprintf(fp_debug, "%f %f pos(%.2f %.2f %.2f %.2f) rot(%.2f %.2f %.2f %.2f %.2f %.2f)\n",
rot_angle_x, rot_angle_y,
sin_half_pos_x, sin_half_pos_y, cos_half_pos_x, cos_half_pos_y,
sin_half_rot_p, sin_half_rot_r, sin_half_rot_h, cos_half_rot_p, cos_half_rot_r, cos_half_rot_h
);
fclose(fp_debug);
} }
#endif
CQuaternion lon (cos_half_pos_y, 0, 0, sin_half_pos_y),
lat (cos_half_pos_x, sin_half_pos_x, 0, 0),
heading (cos_half_rot_h, 0, 0, sin_half_rot_h),
pitch (cos_half_rot_p, sin_half_rot_p, 0, 0),
banking (cos_half_rot_r, 0, sin_half_rot_r, 0);
CQuaternion pb, hpb, lahpb, all;
pb.QuaternionProduct (&pitch, &banking);
hpb.QuaternionProduct (&heading, &pb);
lahpb.QuaternionProduct (&lat, &hpb);
all.QuaternionProduct (&lon, &lahpb);
#ifdef _DEBUG_FRAME_MANAGER
{ FILE *fp_debug;
if(!(fp_debug = fopen("__DDEBUG_AERO_frames.txt", "a")) == NULL)
{
fprintf(fp_debug, "pb(%.2f %.2f %.2f %.2f) hpb(%.2f %.2f %.2f %.2f) lahpb(%.2f %.2f %.2f %.2f) all(%.2f %.2f %.2f %.2f)\n",
pb.w, pb.v.x, pb.v.y, pb.v.z,
hpb.w, hpb.v.x, hpb.v.y, hpb.v.z,
lahpb.w, lahpb.v.x, lahpb.v.y, lahpb.v.z,
all.w, all.v.x, all.v.y, all.v.z
);
fclose(fp_debug);
} }
#endif
CRotMatrix rot;
//rot.Setup (
// arcseconds_lon2radians (pos.lon),
// arcseconds_lat2radians (pos.lat)
//);
rot.QuaternionToRotMat (all);
#ifdef _DEBUG_FRAME_MANAGER
{ FILE *fp_debug;
if(!(fp_debug = fopen("__DDEBUG_AERO_frames.txt", "a")) == NULL)
{
fprintf(fp_debug, " %.5f %.5f %.5f\n %.5f %.5f %.5f\n %.5f %.5f %.5f\n",
rot.m11, rot.m12, rot.m13,
rot.m21, rot.m22, rot.m23,
rot.m31, rot.m32, rot.m33
);
fclose(fp_debug);
} }
#endif
//
tmp_iang.p = safeAsin /*asin*/ (rot.m23);
//tmp_iang.r = safeAtan2 (-rot.m13, rot.m33); // LH
//tmp_iang.h = safeAtan2 (-rot.m21, rot.m22); // LH
tmp_iang.h = safeAtan2 (-rot.m13, rot.m33); // RH
tmp_iang.r = safeAtan2 (-rot.m21, rot.m22); // RH
//rot.TransformItoB (eulerAngle, tmp_iang);
#ifdef _DEBUG_FRAME_MANAGER
{ FILE *fp_debug;
if(!(fp_debug = fopen("__DDEBUG_AERO_frames.txt", "a")) == NULL)
{
fprintf(fp_debug, "euler2iang == lon=%f lat=%f euler(%.2f %.2f %.2f %.2f %.2f %.2f) iang(%.2f %.2f %.2f)\n",
arcseconds_lon2radians (pos.lon),
arcseconds_lat2radians (pos.lat),
eulerAngle.x, eulerAngle.y, eulerAngle.z, eulerAngle.p, eulerAngle.h, eulerAngle.r,
tmp_iang.x, tmp_iang.y, tmp_iang.z
);
fprintf(fp_debug, "======================================================================================\n");
fclose(fp_debug);
} }
#endif
}
// The compute() method does the actual work of the node using the inputs
// of the node to generate its output.
//
// Compute takes two parameters: plug and data.
// - Plug is the the data value that needs to be recomputed
// - Data provides handles to all of the nodes attributes, only these
// handles should be used when performing computations.
//
MStatus motionPathNode::compute( const MPlug& plug, MDataBlock& data )
{
double f;
MStatus status;
// Read the attributes we need from the datablock.
//
double uVal = data.inputValue( uValue ).asDouble();
bool fractionModeVal = data.inputValue( fractionMode ).asBool();
bool followVal = data.inputValue( follow ).asBool();
int frontAxisVal = data.inputValue( frontAxis ).asShort();
int upAxisVal = data.inputValue( upAxis ).asShort();
bool bankVal = data.inputValue( bank ).asBool();
double bankScaleVal = data.inputValue( bankScale ).asDouble();
double bankThresholdVal= data.inputValue( bankThreshold ).asDouble();
double offsetVal = data.inputValue( offset ).asDouble();
double wobbleRateVal = data.inputValue( wobbleRate ).asDouble();
// Make sure the value is fractional.
//
if ( fractionModeVal ) {
f = uVal;
} else {
f = parametricToFractional( uVal, &status );
}
CHECK_MSTATUS_AND_RETURN_IT( status );
// To compute the sample location on the path, first wrap the fraction
// around the start of the the path in case it goes past the end
// to prevent clamping, then compute the sample location on the path.
//
f = wraparoundFractionalValue( f, &status );
CHECK_MSTATUS_AND_RETURN_IT( status );
MPoint location = position( data, f, &status );
CHECK_MSTATUS_AND_RETURN_IT( status );
// Get the orthogonal vectors on the motion path.
//
const MVector worldUp = MGlobal::upAxis();
MVector front, side, up;
CHECK_MSTATUS_AND_RETURN_IT( getVectors( data, f, front, side, up,
&worldUp ) );
// If follow (i.e. rotation) is enabled, check if banking is also
// enabled and if so, bank into the turn.
//
if ( followVal && bankVal ) {
MQuaternion bankQuat = banking( data, f, worldUp,
bankScaleVal, bankThresholdVal, &status );
CHECK_MSTATUS_AND_RETURN_IT( status );
up = up.rotateBy( bankQuat );
side = front ^ up;
}
// Compute the wobble that moves the sphere back and forth as it
// traverses the path.
//
if ( fabs( offsetVal ) > ALMOST_ZERO
&& fabs( wobbleRateVal ) > ALMOST_ZERO ) {
double wobble = offsetVal * sin( TWO_PI * wobbleRateVal * f );
MVector tmp = side * wobble;
location += tmp;
}
// Write the result values to the output plugs.
//
data.outputValue( allCoordinates ).set( location.x, location.y,
location.z );
if ( followVal ) {
MTransformationMatrix resultOrientation = matrix( front, side,
up, frontAxisVal, upAxisVal, &status );
CHECK_MSTATUS_AND_RETURN_IT( status );
MTransformationMatrix::RotationOrder ro
= (MTransformationMatrix::RotationOrder)
( data.inputValue( rotateOrder ).asShort() + 1 );
double rot[3];
status = MTransformationMatrix( resultOrientation ).getRotation(
rot, ro );
CHECK_MSTATUS_AND_RETURN_IT( status );
data.outputValue( rotate ).set( rot[0], rot[1], rot[2] );
}
return( MS::kSuccess );
}