import com.vividsolutions.jts.geom.Geometry; //導入方法依賴的package包/類

/**

* Get the path index beyond which a robot should not navigate, given the {@link TrajectoryEnvelope} of another robot.

* @param te1 The {@link TrajectoryEnvelope} of the leading robot.

* @param te2 The {@link TrajectoryEnvelope} of the yielding robot.

* @param currentPIR1 The current path index of the leading robot.

* @param te1Start The path index

* @param te1End

* @param te2Start

* @return

*/

public int getCriticalPoint(int yieldingRobotID, CriticalSection cs, int leadingRobotCurrentPathIndex) {

//Number of additional path points robot 2 should stay behind robot 1

int TRAILING_PATH_POINTS = 3;

int leadingRobotStart = -1;

int yieldingRobotStart = -1;

int leadingRobotEnd = -1;

int yieldingRobotEnd = -1;

if (cs.getTe1().getRobotID() == yieldingRobotID) {

leadingRobotStart = cs.getTe2Start();

yieldingRobotStart = cs.getTe1Start();

leadingRobotEnd = cs.getTe2End();

yieldingRobotEnd = cs.getTe1End();

}

else {

leadingRobotStart = cs.getTe1Start();

yieldingRobotStart = cs.getTe2Start();

leadingRobotEnd = cs.getTe1End();

yieldingRobotEnd = cs.getTe2End();

}

TrajectoryEnvelope leadingRobotTE = null;

TrajectoryEnvelope yieldingRobotTE = null;

if (cs.getTe1().getRobotID() == yieldingRobotID) {

leadingRobotTE = cs.getTe2();

yieldingRobotTE = cs.getTe1();

}

else {

leadingRobotTE = cs.getTe1();

yieldingRobotTE = cs.getTe2();

}

if (leadingRobotCurrentPathIndex <= leadingRobotStart) {

return Math.max(0, yieldingRobotStart-TRAILING_PATH_POINTS);

}

//Compute sweep of robot 1's footprint from current position to LOOKAHEAD

Pose leadingRobotPose = leadingRobotTE.getTrajectory().getPose()[leadingRobotCurrentPathIndex];

Geometry leadingRobotInPose = TrajectoryEnvelope.getFootprint(leadingRobotTE.getFootprint(), leadingRobotPose.getX(), leadingRobotPose.getY(), leadingRobotPose.getTheta());

for (int i = leadingRobotCurrentPathIndex+1; i <= leadingRobotEnd; i++) {

Pose leadingRobotNextPose = leadingRobotTE.getTrajectory().getPose()[i];

leadingRobotInPose = leadingRobotInPose.union(TrajectoryEnvelope.getFootprint(leadingRobotTE.getFootprint(), leadingRobotNextPose.getX(), leadingRobotNextPose.getY(), leadingRobotNextPose.getTheta()));

}

//Return pose at which yielding robot should stop given driving robot's projected sweep

for (int i = yieldingRobotStart; i < yieldingRobotEnd; i++) {

Pose yieldingRobotPose = yieldingRobotTE.getTrajectory().getPose()[i];

Geometry yieldingRobotInPose = TrajectoryEnvelope.getFootprint(yieldingRobotTE.getFootprint(), yieldingRobotPose.getX(), yieldingRobotPose.getY(), yieldingRobotPose.getTheta());

if (leadingRobotInPose.intersects(yieldingRobotInPose)) {

return Math.max(0, i-TRAILING_PATH_POINTS);

}

}

//The only situation where the above has not returned is when robot 2 should

//stay "parked", therefore wait at index 0

return Math.max(0, yieldingRobotStart-TRAILING_PATH_POINTS);

}