private static SystemState GetSystemState(ArrayList arr, int index)
{
int posStart = index * 3;
double altitude = (double)arr[posStart];
double latitude = (double)arr[posStart + 1];
double longitude = (double)arr[posStart + 2];
if (index == 1)
{
longitude = longitude - .0000006;
}
if (index == 2)
{
longitude = longitude - .0000008;
}
CoordPosition pos = new CoordPosition(altitude, latitude, longitude);
int rotStart = 9 + 4 * index;
double w = (double)arr[rotStart];
double x = (double)arr[rotStart + 1];
double y = (double)arr[rotStart + 2];
double z = (double)arr[rotStart + 3];
//float x1 = (float)System.Math.Atan2(2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z);
//float y1 = (float)System.Math.Atan2(2 * x * w - 2 * y * z, 1 - 2 * x * x - 2 * z * z);
//float z1 = (float)System.Math.Asin(2 * x * y + 2 * z * w);
//Vector3 rotation = new Vector3(x1, y1, z1);
Quaternion rotation1 = new Quaternion((float)x, (float)y, (float)z, (float)w);
Quaternion rotation = rotation1 * Quaternion.Euler(-101.6f, 0.9f, -.9f);
SystemState state = new SystemState(rotation, pos);
return(state);
}
public static extern int HIMC_CircAbs(int nCtrlID, int nGroupID, CenterPosition center_pos, NormalVector normal_vector, int turns, CoordPosition target_pos, MotionProfile motion_profile, CoordSystem coord_sys, MotionBufferMode buf_mode, MotionTransitionMode trans_mode, double dTransPar);
public static extern int HIMC_LinRel(int nCtrlID, int nGroupID, CoordPosition relative_dist, MotionProfile motion_profile, CoordSystem coord_sys, MotionBufferMode buf_mode, MotionTransitionMode trans_mode, double dTransPar);
public static extern int HIMC_LinAbs(int nCtrlID, int nGroupID, CoordPosition target_pos, MotionProfile motion_profile, CoordSystem coord_sys, MotionBufferMode buf_mode, MotionTransitionMode trans_mode, double dTransPar);
public SystemState(Quaternion Rotation, CoordPosition Position)
{
this.Rotation = Rotation;
this.Position = Position;
}
