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); } }