import com.vividsolutions.jts.geom.Geometry; //導(dǎo)入方法依賴的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);

}

總結(jié)

以上是生活随笔為你收集整理的java union方法参数_Java Geometry.union方法代碼示例的全部?jī)?nèi)容，希望文章能夠幫你解決所遇到的問(wèn)題。

如果覺得生活随笔網(wǎng)站內(nèi)容還不錯(cuò)，歡迎將生活随笔推薦給好友。