1+ /*
2+ * Copyright (c) 2004-2026 Universidade do Porto - Faculdade de Engenharia
3+ * Laboratório de Sistemas e Tecnologia Subaquática (LSTS)
4+ * All rights reserved.
5+ * Rua Dr. Roberto Frias s/n, sala I203, 4200-465 Porto, Portugal
6+ *
7+ * This file is part of Neptus, Command and Control Framework.
8+ *
9+ * Commercial Licence Usage
10+ * Licencees holding valid commercial Neptus licences may use this file
11+ * in accordance with the commercial licence agreement provided with the
12+ * Software or, alternatively, in accordance with the terms contained in a
13+ * written agreement between you and Universidade do Porto. For licensing
14+ * terms, conditions, and further information contact lsts@fe.up.pt.
15+ *
16+ * Modified European Union Public Licence - EUPL v.1.1 Usage
17+ * Alternatively, this file may be used under the terms of the Modified EUPL,
18+ * Version 1.1 only (the "Licence"), appearing in the file LICENCE.md
19+ * included in the packaging of this file. You may not use this work
20+ * except in compliance with the Licence. Unless required by applicable
21+ * law or agreed to in writing, software distributed under the Licence is
22+ * distributed on an "AS IS" basis, WITHOUT WARRANTIES OR CONDITIONS OF
23+ * ANY KIND, either express or implied. See the Licence for the specific
24+ * language governing permissions and limitations at
25+ * https://github.com/LSTS/neptus/blob/develop/LICENSE.md
26+ * and http://ec.europa.eu/idabc/eupl.html.
27+ *
28+ * For more information please see <http://lsts.fe.up.pt/neptus>.
29+ *
30+ * Author: jcordeiro
31+ * January 06, 2026
32+ */
33+ package pt .lsts .neptus .mra .replay ;
34+
35+ import java .awt .BasicStroke ;
36+ import java .awt .Color ;
37+ import java .awt .Graphics2D ;
38+ import java .awt .Polygon ;
39+ import java .awt .RenderingHints ;
40+ import java .awt .geom .AffineTransform ;
41+ import java .awt .geom .Point2D ;
42+
43+ import pt .lsts .imc .EstimatedState ;
44+ import pt .lsts .imc .IMCMessage ;
45+ import pt .lsts .imc .lsf .LsfIndex ;
46+ import pt .lsts .neptus .mra .importers .IMraLogGroup ;
47+ import pt .lsts .neptus .plugins .PluginDescription ;
48+ import pt .lsts .neptus .renderer2d .StateRenderer2D ;
49+ import pt .lsts .neptus .types .coord .LocationType ;
50+ import pt .lsts .neptus .NeptusLog ;
51+
52+ @ PluginDescription (icon = "images/menus/compass.png" , name = "Desired Heading" )
53+ public class DesiredHeadingLayer implements LogReplayLayer {
54+ private static final Color ARROW_COLOR = new Color (255 , 255 , 0 , 200 );
55+ private static final int ARROW_LENGTH_PX = 40 ;
56+ private static final int ARROW_OFFSET_PX = 20 ;
57+ private static final int ARROW_GAP_PX = 4 ;
58+ private static final int ARROW_HEAD_LEN = 10 ;
59+ private static final int ARROW_HEAD_WIDTH = 6 ;
60+
61+ private LocationType currentPosition ;
62+ private double currentHeading = 0 ;
63+ private boolean hasPosition = false ;
64+ private LsfIndex index ;
65+
66+ @ Override
67+ public void paint (Graphics2D g , StateRenderer2D renderer ) {
68+ if (!hasPosition ) {
69+ return ;
70+ }
71+
72+ Graphics2D g2 = (Graphics2D ) g .create ();
73+ try {
74+ g2 .setColor (ARROW_COLOR );
75+ g2 .setRenderingHint (RenderingHints .KEY_ANTIALIASING , RenderingHints .VALUE_ANTIALIAS_ON );
76+ g2 .setStroke (new BasicStroke (2.5f ));
77+
78+ Point2D vehiclePoint = renderer .getScreenPosition (currentPosition );
79+
80+ double headingRad = Math .toRadians (currentHeading );
81+
82+ double ux = Math .sin (headingRad );
83+ double uy = -Math .cos (headingRad );
84+
85+ Point2D startPoint = new Point2D .Double (
86+ vehiclePoint .getX () + ux * ARROW_OFFSET_PX ,
87+ vehiclePoint .getY () + uy * ARROW_OFFSET_PX
88+ );
89+
90+ Point2D endPoint = new Point2D .Double (
91+ startPoint .getX () + ux * ARROW_LENGTH_PX ,
92+ startPoint .getY () + uy * ARROW_LENGTH_PX
93+ );
94+
95+ g2 .drawLine (
96+ (int ) startPoint .getX (),
97+ (int ) startPoint .getY (),
98+ (int ) endPoint .getX (),
99+ (int ) endPoint .getY ()
100+ );
101+
102+ Point2D arrowTip = new Point2D .Double (
103+ endPoint .getX () + ux * ARROW_GAP_PX ,
104+ endPoint .getY () + uy * ARROW_GAP_PX
105+ );
106+ double angle = Math .atan2 (uy , ux ) - Math .PI /2 ;
107+ drawArrowHead (g2 , arrowTip , angle );
108+ } finally {
109+ g2 .dispose ();
110+ }
111+ }
112+
113+ private void drawArrowHead (Graphics2D g2 , Point2D point , double angle ) {
114+ AffineTransform old = g2 .getTransform ();
115+
116+ g2 .translate (point .getX (), point .getY ());
117+ g2 .rotate (angle );
118+
119+ Polygon arrowHead = new Polygon ();
120+ arrowHead .addPoint (0 , 0 );
121+ arrowHead .addPoint (-(int )(ARROW_HEAD_WIDTH ), (int ) -ARROW_HEAD_LEN );
122+ arrowHead .addPoint ((int )(ARROW_HEAD_WIDTH ), (int ) -ARROW_HEAD_LEN );
123+
124+ g2 .fill (arrowHead );
125+ g2 .setTransform (old );
126+ }
127+
128+ @ Override
129+ public void onMessage (IMCMessage message ) {
130+ if ("DesiredHeading" .equals (message .getAbbrev ())) {
131+ currentHeading = Math .toDegrees (message .getDouble ("value" ));
132+ }
133+ else if ("EstimatedState" .equals (message .getAbbrev ())) {
134+ try {
135+ EstimatedState state = (EstimatedState ) message ;
136+ currentPosition = new LocationType (
137+ Math .toDegrees (state .getLat ()),
138+ Math .toDegrees (state .getLon ())
139+ );
140+ currentPosition .setDepth (state .getDepth ());
141+ currentPosition .translatePosition (state .getX (), state .getY (), state .getZ ());
142+ hasPosition = true ;
143+ } catch (Exception e ) {
144+ NeptusLog .pub ().error ("Error processing EstimatedState" , e );
145+ }
146+ }
147+ }
148+
149+ @ Override
150+ public void parse (IMraLogGroup source ) {
151+ this .index = source .getLsfIndex ();
152+ }
153+
154+ @ Override
155+ public String [] getObservedMessages () {
156+ return new String []{"DesiredHeading" , "EstimatedState" };
157+ }
158+
159+ @ Override
160+ public boolean canBeApplied (IMraLogGroup source , Context context ) {
161+ return true ;
162+ }
163+
164+ @ Override
165+ public String getName () {
166+ return "Desired Heading" ;
167+ }
168+
169+ @ Override
170+ public boolean getVisibleByDefault () {
171+ return false ;
172+ }
173+
174+ @ Override
175+ public void cleanup () {
176+ currentPosition = null ;
177+ hasPosition = false ;
178+ }
179+ }
0 commit comments