001/* *********************************************************************** * 002 * project: org.matsim.* 003 * OTFLaneModelBuilder 004 * * 005 * *********************************************************************** * 006 * * 007 * copyright : (C) 2012 by the members listed in the COPYING, * 008 * LICENSE and WARRANTY file. * 009 * email : info at matsim dot org * 010 * * 011 * *********************************************************************** * 012 * * 013 * This program is free software; you can redistribute it and/or modify * 014 * it under the terms of the GNU General Public License as published by * 015 * the Free Software Foundation; either version 2 of the License, or * 016 * (at your option) any later version. * 017 * See also COPYING, LICENSE and WARRANTY file * 018 * * 019 * *********************************************************************** */ 020package org.matsim.lanes; 021 022import java.awt.geom.Point2D; 023import java.awt.geom.Point2D.Double; 024import java.util.List; 025import java.util.Map; 026 027import org.matsim.api.core.v01.Coord; 028import org.matsim.api.core.v01.Id; 029import org.matsim.api.core.v01.network.Link; 030import org.matsim.core.utils.collections.Tuple; 031import org.matsim.core.utils.geometry.CoordinateTransformation; 032import org.matsim.vis.snapshotwriters.SnapshotLinkWidthCalculator; 033import org.matsim.vis.snapshotwriters.VisLink; 034import org.matsim.vis.vecmathutils.VectorUtils; 035 036/** 037 * @author dgrether 038 */ 039public final class VisLaneModelBuilder { 040 041// private static final Logger log = Logger.getLogger(VisLaneModelBuilder.class); 042 043 public void recalculatePositions(VisLinkWLanes linkData, SnapshotLinkWidthCalculator linkWidthCalculator) { 044// log.error("recalculatePositions..."); 045 double linkWidth = linkWidthCalculator.calculateLinkWidth(linkData.getNumberOfLanes()) ; 046 linkData.setLinkWidth(linkWidth); 047 Point2D.Double linkStartCenter = this.calculatePointOnLink(linkData, 0.0, 0.5); 048 linkData.setLinkStartCenterPoint(linkStartCenter); 049 if (linkData.getLaneData() == null || linkData.getLaneData().isEmpty()){ 050 //Calculate end point center 051 double x = linkData.getLinkEnd().x + (0.5 * linkWidth * linkData.getLinkOrthogonalVector().x); 052 double y = linkData.getLinkEnd().y + (0.5 * linkWidth * linkData.getLinkOrthogonalVector().y); 053 linkData.setLinkEndCenterPoint(new Point2D.Double(x, y)); 054// log.error("link " + linkData.getLinkId() + " without lanes starts at " + linkData.getLinkStartCenterPoint() + " and ends at " + x + " " + y + " but " + linkData.getLinkEnd()); 055 } 056 else { 057 double numberOfLinkParts = (2 * linkData.getMaximalAlignment()) + 2; 058// log.error("link with lanes " + linkData.getLinkId() + " starts at " + linkData.getLinkStart() + " and ends at " + linkData.getLinkEnd()); 059 for (VisLane lane : linkData.getLaneData().values()){ 060 double horizontalFraction = 0.5 - (lane.getAlignment() / numberOfLinkParts); 061 Point2D.Double laneStart = calculatePointOnLink(linkData, lane.getStartPosition(), horizontalFraction); 062 Point2D.Double laneEnd = calculatePointOnLink(linkData, lane.getEndPosition(), horizontalFraction); 063 lane.setStartEndPoint(laneStart, laneEnd); 064// log.error("lane " + lane.getId() + " starts at " + laneStart + " ends at " + laneEnd); 065 if (lane.getNumberOfLanes() >= 2.0){ 066 double noLanesFloor = Math.floor(lane.getNumberOfLanes()); 067 double laneOffset = - noLanesFloor / 2 * linkWidthCalculator.getLaneWidth(); 068 if (noLanesFloor % 2 == 0){ 069 laneOffset = laneOffset + (linkWidthCalculator.getLaneWidth()/2); 070 } 071 for (int i = 1; i <= noLanesFloor; i++){ 072 Point2D.Double drivingLaneStart = this.calcPoint(laneStart, linkData.getLinkOrthogonalVector(), laneOffset); 073 Point2D.Double drivingLaneEnd = this.calcPoint(laneEnd, linkData.getLinkOrthogonalVector(), laneOffset); 074 lane.addDrivingLane(i, drivingLaneStart, drivingLaneEnd); 075 laneOffset = laneOffset + linkWidthCalculator.getLaneWidth(); 076 } 077 } 078 } 079 } 080 } 081 082 private Point2D.Double calculatePointOnLink(final VisLinkWLanes laneLinkData, final double position, final double horizontalFraction) { 083 Point2D.Double lenghtPoint = this.calcPoint(laneLinkData.getLinkStart(), laneLinkData.getNormalizedLinkVector(), position); 084 return this.calcPoint(lenghtPoint, laneLinkData.getLinkOrthogonalVector(), horizontalFraction * laneLinkData.getLinkWidth()); 085 } 086 087 public Point2D.Double calcPoint(Point2D.Double start, Point2D.Double vector, double distance){ 088 double x = start.getX() + (distance * vector.x); 089 double y = start.getY() + (distance * vector.y); 090 return new Point2D.Double(x, y); 091 } 092 093 private VisLane createVisLane(ModelLane qlane, double linkLength, double linkScale, double linkLengthCorrectionFactor) { 094 String id = qlane.getLaneData().getId().toString(); 095 double startPosition = (linkLength - qlane.getLaneData().getStartsAtMeterFromLinkEnd()) * linkScale * linkLengthCorrectionFactor; 096 double endPosition = startPosition + (qlane.getLength() * linkScale * linkLengthCorrectionFactor); 097// log.error("lane " + qlane.getId() + " starts at: " + startPosition + " and ends at : " +endPosition); 098 int alignment = qlane.getLaneData().getAlignment(); 099 VisLane lane = new VisLane(id); 100 lane.setStartPosition(startPosition); 101 lane.setEndPosition(endPosition); 102 lane.setAlignment(alignment); 103 lane.setNumberOfLanes(qlane.getLaneData().getNumberOfRepresentedLanes()); 104 return lane; 105 } 106 107 108 109 public void connect(Map<String, VisLinkWLanes> otfNetwork){ 110 for (VisLinkWLanes otfLink : otfNetwork.values()){ 111 if (otfLink.getLaneData() == null || otfLink.getLaneData().isEmpty()){ 112 if (otfLink.getToLinkIds() != null){ 113 for (String toLinkId : otfLink.getToLinkIds()){ 114 VisLinkWLanes toLink = otfNetwork.get(toLinkId); 115 otfLink.addToLink(toLink); 116 } 117 } 118 } 119 else { 120 for (VisLane otfLane : otfLink.getLaneData().values()){ 121 if (otfLane.getToLinkIds() != null) { 122 for (String toLinkId : otfLane.getToLinkIds()){ 123 VisLinkWLanes toLink = otfNetwork.get(toLinkId); 124 otfLane.addToLink(toLink); 125 } 126 } 127 } 128 } 129 } 130 } 131 132 public VisLinkWLanes createVisLinkLanes(CoordinateTransformation transform, VisLink link, double nodeOffsetMeter, List<ModelLane> lanes) { 133// log.error(""); 134// log.error("link " + link.getLink().getId() + " ... "); 135// log.debug(" fromNode: " + link.getLink().getFromNode().getId() + " coord: " + link.getLink().getFromNode().getCoord()); 136// log.debug(" toNode: " + link.getLink().getToNode().getId() + " coord: " + link.getLink().getToNode().getCoord()); 137 Coord linkStartCoord = transform.transform(link.getLink().getFromNode().getCoord()); 138 Coord linkEndCoord = transform.transform(link.getLink().getToNode().getCoord()); 139 Point2D.Double linkStart = new Point2D.Double(linkStartCoord.getX(), linkStartCoord.getY()); 140 Point2D.Double linkEnd = new Point2D.Double(linkEndCoord.getX(), linkEndCoord.getY()); 141 142 //calculate length and normal 143 Point2D.Double deltaLink = new Point2D.Double(linkEnd.x - linkStart.x, linkEnd.y - linkStart.y); 144 double euclideanLinkLength = this.calculateEuclideanLinkLength(deltaLink); 145 //calculate the correction factor if real link length is different than euclidean distance 146 double linkLengthCorrectionFactor = euclideanLinkLength / link.getLink().getLength(); 147 Point2D.Double deltaLinkNorm = new Point2D.Double(deltaLink.x / euclideanLinkLength, deltaLink.y / euclideanLinkLength); 148 Point2D.Double normalizedOrthogonal = new Point2D.Double(deltaLinkNorm.y, - deltaLinkNorm.x); 149 150 //first calculate the scale of the link based on the node offset, i.e. the link will be shortened at the beginning and the end 151 double linkScale = 1.0; 152 if ((euclideanLinkLength * 0.2) > (2.0 * nodeOffsetMeter)){ // 2* nodeoffset is more than 20% 153 linkScale = (euclideanLinkLength - (2.0 * nodeOffsetMeter)) / euclideanLinkLength; 154 } 155 else { // use 80 % as euclidean length 156 linkScale = euclideanLinkLength * 0.8 / euclideanLinkLength; 157 } 158 159 //scale the link 160 Tuple<Double, Double> scaledLink = VectorUtils.scaleVector(linkStart, linkEnd, linkScale); 161 Point2D.Double scaledLinkEnd = scaledLink.getSecond(); 162 Point2D.Double scaledLinkStart = scaledLink.getFirst(); 163// log.error("scaledLinkStart: " + scaledLinkStart + " end: " + scaledLinkEnd); 164 165 VisLinkWLanes lanesLinkData = new VisLinkWLanes(link.getLink().getId().toString()); 166 lanesLinkData.setLinkStartEndPoint(scaledLinkStart, scaledLinkEnd); 167 lanesLinkData.setNormalizedLinkVector(deltaLinkNorm); 168 lanesLinkData.setLinkOrthogonalVector(normalizedOrthogonal); 169 lanesLinkData.setNumberOfLanes(link.getLink().getNumberOfLanes()); 170 171 if (lanes != null){ 172 int maxAlignment = 0; 173 for (ModelLane lane : lanes){ 174 VisLane visLane = this.createVisLane(lane, link.getLink().getLength(), linkScale, linkLengthCorrectionFactor); 175 lanesLinkData.addLaneData(visLane); 176 if (visLane.getAlignment() > maxAlignment) { 177 maxAlignment = visLane.getAlignment(); 178 } 179 } 180 lanesLinkData.setMaximalAlignment(maxAlignment); 181 182 //connect the lanes 183 for (ModelLane lane : lanes){ 184 VisLane otfLane = lanesLinkData.getLaneData().get(lane.getLaneData().getId().toString()); 185 if (lane.getToLanes() == null || lane.getToLanes().isEmpty()){ 186 for (Id<Link> id : lane.getLaneData().getToLinkIds()){ 187 otfLane.addToLinkId(id.toString()); 188 } 189 } 190 else { 191 for (ModelLane toLane : lane.getToLanes()){ 192 VisLane otfToLane = lanesLinkData.getLaneData().get(toLane.getLaneData().getId().toString()); 193 otfLane.addToLane(otfToLane); 194 } 195 } 196 } 197 } 198 else { 199 for (Id<Link> id : link.getLink().getToNode().getOutLinks().keySet()){ 200 lanesLinkData.addToLinkId(id.toString()); 201 } 202 } 203 204 return lanesLinkData; 205 } 206 207 208 209 private double calculateEuclideanLinkLength(Point2D.Double deltaLink) { 210 return Math.sqrt(Math.pow(deltaLink.x, 2) + Math.pow(deltaLink.y, 2)); 211 } 212 213}