void moveTo(Vector3D ROID_START, Vector3D ROID_END)
{
//Setup Of Common Variables
Vector3D DronePosition = RC.GetPosition();
Vector3D Drone_To_Target = Vector3D.Normalize(ROID_END - DronePosition);
//Generates XYZ Vectors
Vector3D X_ADD = Vector3D.Normalize(ROID_END - ROID_START); //Characteristic 'Forward' vector
Vector3D Y_ADD = Vector3D.CalculatePerpendicularVector(X_ADD); //Characteristic 'Left' vector
Vector3D Z_ADD = Vector3D.Cross(X_ADD, Y_ADD); //Characteristic 'Up' vector
//设置起点
Vector3D CurrentVectorStart = ROID_START;
//设置终点
Vector3D CurrentVectorEnd = ROID_END; //Accounts for small input
double dis = Vector3D.Distance(CurrentVectorStart, DronePosition);
//Inputs To Autopilot Function
double RollReqt = (float)(0.6 * (Vector3D.Dot(Z_ADD, RC.WorldMatrix.Down)));
GyroTurn6(X_ADD * 999999999999999999, RotationalSensitvity, GYRO, RC, RollReqt, PrecisionMaxAngularVel);
if (dis > 20 && isAutoPilot)
{
RC_Manager(CurrentVectorStart, RC, false);
if (dis < 30)
{
isAutoPilot = false;
}
print("方式：自动驾驶");
}
else
{
print("方式：程序控制");
Vector_Thrust_Manager(CurrentVectorStart, CurrentVectorEnd, RC.GetPosition(), 1, 0.5, RC);
}
}
void towards(Vector3D ROID_START, Vector3D ROID_CENTRE, double ROID_DIAMETER, double SHIPSIZE, bool Reset)
{
//Setup Of Common Variables
Vector3D DronePosition = RC.GetPosition();
Vector3D Drone_To_Target = Vector3D.Normalize(ROID_CENTRE - DronePosition);
//Generates XYZ Vectors
Vector3D X_ADD = Vector3D.Normalize(ROID_CENTRE - ROID_START); //Characteristic 'Forward' vector
Vector3D Y_ADD = Vector3D.CalculatePerpendicularVector(X_ADD); //Characteristic 'Left' vector
Vector3D Z_ADD = Vector3D.Cross(X_ADD, Y_ADD); //Characteristic 'Up' vector
//Generates Array Of Starting Vectors
int Steps = (int)((ROID_DIAMETER * 0.3) / SHIPSIZE); //How many horizontal passes of the ship are required to eat the roid
double StepSize = SHIPSIZE; //How big are those passes
//设置起点
Vector3D CurrentVectorStart = new Vector3D();
//设置终点
Vector3D CurrentVectorEnd = CurrentVectorStart + X_ADD * (((ROID_CENTRE - ROID_START).Length() - ROID_DIAMETER / 2) + ROID_DIAMETER * 0.8); //Accounts for small input
double dis = Vector3D.Distance(CurrentVectorStart, DronePosition);
//Inputs To Autopilot Function
double RollReqt = (float)(0.6 * (Vector3D.Dot(Z_ADD, RC.WorldMatrix.Down)));
GyroTurn6(X_ADD * 999999999999999999, RotationalSensitvity, GYRO, RC, RollReqt, PrecisionMaxAngularVel);
if (dis > 20 && isAutoPilot)
{
RC_Manager(CurrentVectorStart, RC, false);
if (dis < 30)
{
isAutoPilot = false;
}
print("方式：自动驾驶");
}
else
{
print("方式：程序控制");
Vector_Thrust_Manager(CurrentVectorStart, CurrentVectorEnd, RC.GetPosition(), 1, 0.5, RC);
}
}
