| /******************************************************************************* |
| * Copyright (c) 2018 Agence spatiale canadienne / Canadian Space Agency |
| * All rights reserved. This program and the accompanying materials |
| * are made available under the terms of the Eclipse Public License v1.0 |
| * which accompanies this distribution, and is available at |
| * http://www.eclipse.org/legal/epl-v10.html |
| * |
| * Contributors: |
| * Pierre Allard, |
| * Regent L'Archeveque, |
| * Sebastien Gemme - initial API and implementation |
| * |
| * SPDX-License-Identifier: EPL-1.0 |
| * |
| *******************************************************************************/ |
| package org.eclipse.apogy.addons.mobility.pathplanners.graph.impl; |
| |
| import java.util.Iterator; |
| |
| import org.eclipse.apogy.addons.mobility.pathplanners.ExclusionZone; |
| import org.eclipse.apogy.common.geometry.data3d.CartesianPolygon; |
| import org.eclipse.apogy.common.geometry.data3d.CartesianPositionCoordinates; |
| |
| public class ExclusionZonesCostFunctionCustomImpl extends ExclusionZonesCostFunctionImpl { |
| |
| @Override |
| public double getCost(CartesianPolygon from, CartesianPolygon to) { |
| double cost = 0.0; |
| |
| CartesianPositionCoordinates fromCenter = from.getCentroid(); |
| CartesianPositionCoordinates toCenter = to.getCentroid(); |
| |
| // Goes through the list of ExclusionZone and check for crossing of |
| // the line connecting the centroid of the from polygon to the centroid |
| // of the to polygon. |
| Iterator<ExclusionZone> zones = getExclusionZones().iterator(); |
| while (zones.hasNext() && (cost != Double.POSITIVE_INFINITY)) { |
| ExclusionZone zone = zones.next(); |
| |
| if (zone.intersects(fromCenter, toCenter)) { |
| cost = Double.POSITIVE_INFINITY; |
| } |
| } |
| |
| return cost; |
| } |
| } // ExclusionZonesCostFunctionImpl |
