コード例 #1
0
ファイル: FrameManager.cpp プロジェクト: FlySimvol/FlyLegacy
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
}
コード例 #2
0
// 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 );
}