diff --git a/README.md b/README.md
index 4b474f9e1ced7e2ec9380b803dc213f6b759cde8..56f7fb5b39cd6ef9d30cfa622ec4408e4cddd580 100644
--- a/README.md
+++ b/README.md
@@ -48,10 +48,10 @@ Connection settings
"accuracy": 1
},
"orientation": {
- "x": 1,
- "y": 0.5,
- "z": 1,
- "w": 1.5
+ "x": 0,
+ "y": 0,
+ "z": -0.7071,
+ "w": 0.7071
},
"extractedAttributes": {
"batteryChargeLevel": 70,
diff --git a/package-lock.json b/package-lock.json
index e3efe91d06e9059d459d9c3958a7676d30b4fcd6..df7e915ed2731d0d84022fd35d6f29df826fd846 100644
--- a/package-lock.json
+++ b/package-lock.json
@@ -22,6 +22,7 @@
"apexcharts": "^3.29.0",
"express": "^4.17.1",
"leaflet": "^1.7.1",
+ "leaflet-arrowheads": "^1.4.0",
"moment": "^2.24.0",
"ng-apexcharts": "^1.5.12",
"ngx-mqtt": "^9.0.5",
@@ -8578,6 +8579,23 @@
"resolved": "https://registry.npmjs.org/leaflet/-/leaflet-1.7.1.tgz",
"integrity": "sha512-/xwPEBidtg69Q3HlqPdU3DnrXQOvQU/CCHA1tcDQVzOwm91YMYaILjNp7L4Eaw5Z4sOYdbBz6koWyibppd8Zqw=="
},
+ "node_modules/leaflet-arrowheads": {
+ "version": "1.4.0",
+ "resolved": "https://registry.npmjs.org/leaflet-arrowheads/-/leaflet-arrowheads-1.4.0.tgz",
+ "integrity": "sha512-aIjsmoWe1VJXaGOpKpS6E8EzN2vpx3GGCNP/FxQteLVzAg5xMID7elf9hj/1CWLJo8FuGRjSvKkUQDj7mocrYA==",
+ "dependencies": {
+ "leaflet": "^1.7.1",
+ "leaflet-geometryutil": "^0.10.0"
+ }
+ },
+ "node_modules/leaflet-geometryutil": {
+ "version": "0.10.0",
+ "resolved": "https://registry.npmjs.org/leaflet-geometryutil/-/leaflet-geometryutil-0.10.0.tgz",
+ "integrity": "sha512-4OGu2OnpHLx+7QpOY6NatrAiSWWrP/Z3HOM9ZsmQ0JWIzAytFS6SLUnKHVjwpgRzPUpV/6w0b4Fh24pVIgqFHw==",
+ "dependencies": {
+ "leaflet": "^1.6.0"
+ }
+ },
"node_modules/less": {
"version": "4.1.1",
"resolved": "https://registry.npmjs.org/less/-/less-4.1.1.tgz",
@@ -22939,6 +22957,23 @@
"resolved": "https://registry.npmjs.org/leaflet/-/leaflet-1.7.1.tgz",
"integrity": "sha512-/xwPEBidtg69Q3HlqPdU3DnrXQOvQU/CCHA1tcDQVzOwm91YMYaILjNp7L4Eaw5Z4sOYdbBz6koWyibppd8Zqw=="
},
+ "leaflet-arrowheads": {
+ "version": "1.4.0",
+ "resolved": "https://registry.npmjs.org/leaflet-arrowheads/-/leaflet-arrowheads-1.4.0.tgz",
+ "integrity": "sha512-aIjsmoWe1VJXaGOpKpS6E8EzN2vpx3GGCNP/FxQteLVzAg5xMID7elf9hj/1CWLJo8FuGRjSvKkUQDj7mocrYA==",
+ "requires": {
+ "leaflet": "^1.7.1",
+ "leaflet-geometryutil": "^0.10.0"
+ }
+ },
+ "leaflet-geometryutil": {
+ "version": "0.10.0",
+ "resolved": "https://registry.npmjs.org/leaflet-geometryutil/-/leaflet-geometryutil-0.10.0.tgz",
+ "integrity": "sha512-4OGu2OnpHLx+7QpOY6NatrAiSWWrP/Z3HOM9ZsmQ0JWIzAytFS6SLUnKHVjwpgRzPUpV/6w0b4Fh24pVIgqFHw==",
+ "requires": {
+ "leaflet": "^1.6.0"
+ }
+ },
"less": {
"version": "4.1.1",
"resolved": "https://registry.npmjs.org/less/-/less-4.1.1.tgz",
diff --git a/package.json b/package.json
index 679d17789dca33e72034098ad6ed918d60d714ee..94b2b289df4522981316ecdf25f6b3634bdad40c 100644
--- a/package.json
+++ b/package.json
@@ -24,6 +24,7 @@
"apexcharts": "^3.29.0",
"express": "^4.17.1",
"leaflet": "^1.7.1",
+ "leaflet-arrowheads": "^1.4.0",
"moment": "^2.24.0",
"ng-apexcharts": "^1.5.12",
"ngx-mqtt": "^9.0.5",
diff --git a/src/app/components/map/map.component.ts b/src/app/components/map/map.component.ts
index e87b0c66581365e2538d6cf8f254a0b5aac2dd3b..4d67808ae7e04f7d5d53255f40fdbe5047046ed2 100644
--- a/src/app/components/map/map.component.ts
+++ b/src/app/components/map/map.component.ts
@@ -2,10 +2,11 @@ import { Component, OnInit } from '@angular/core';
import { Subscription } from 'rxjs';
import * as L from "leaflet";
+import 'leaflet-arrowheads';
import { IMqttMessage, MqttService } from 'ngx-mqtt';
import { environment, MarkerColorMap } from 'src/environments/environment';
-import {Position, PositionUpdate, RelativePos, WGS84} from 'src/app/model/base-model';
+import {Position, PositionUpdate, RelativePos, Quaternion, WGS84} from 'src/app/model/base-model';
@Component({
@@ -71,17 +72,49 @@ export class MapComponent implements OnInit {
if (desc.extractedAttributes) {
props = `<p> \
<h3>${key}</h3>
- <strong>Charge</strong>: ${desc.extractedAttributes.batteryChargeLevel}% <br>\
- <strong>Items</strong>: ${desc.extractedAttributes.loadedItems}\
+ <strong>Charge</strong>: ${desc.extractedAttributes?.batteryChargeLevel}% <br>\
+ <strong>Items</strong>: ${desc.extractedAttributes?.loadedItems} <br>\
+ <strong>Errors</strong>: ${desc.extractedAttributes?.errors}\
</p>`
} else {
props = `<p><h3>${key}</h3></p>`
}
- this.addMarker(key, pos, props)
+ // Rotaion
+ let quter = desc.orientation
+ // if (quter) {
+ // // console.log("Init:", orient)
+ // let orient = this.calcOrient(quter.w, quter.x, quter.y, quter.z)
+ // // console.log("Orient:", orient)
+ // let orientPoint: RelativePos = {
+ // "x": orient[0] + pos.point.x,
+ // "y": orient[1] + pos.point.y,
+ // "z": orient[2] + pos.point.z
+ // }
+
+ // let globPointOrient = this.ref2root(orientPoint)
+
+ // if ('x' in pos.point) {
+ // var globPos = this.ref2root(pos.point)
+ // } else {
+ // var globPos = L.latLng([pos.point.latitude, pos.point.longitude]);
+ // }
+
+ // // add arrow
+ // var arrow = L.polyline([globPos, globPointOrient], {color: 'grey'}).arrowheads(
+ // {
+ // fill: true,
+ // // size: '30%',
+ // color: 'grey'
+ // }
+ // ).addTo(this.map);
+
+ // }
+
+ this.addMarker(key, pos, props, quter)
}
- addMarker(key: string, pos: Position, popup: string) {
+ addMarker(key: string, pos: Position, popup: string, quter: Quaternion) {
let point = pos.point
if ('x' in point) {
@@ -114,6 +147,44 @@ export class MapComponent implements OnInit {
L.circle(globPos, pos.accuracy, { color: markerConf.circle}
).addTo(this.markOverlays[key]);
}
+
+ // --- Orientation
+ if (quter && 'x' in point) {
+ let orient = this.calcOrient(quter.w, quter.x, quter.y, quter.z)
+ let orientPoint: RelativePos = {
+ "x": orient[0] + point.x,
+ "y": orient[1] + point.y,
+ "z": orient[2]
+ }
+ let globPointOrient = this.ref2root(orientPoint)
+
+ // add arrow
+ var arrow = L.polyline([globPos, globPointOrient], {color: 'grey'}).arrowheads(
+ {
+ fill: true,
+ // size: '30%',
+ color: 'grey'
+ }
+ );
+ arrow.addTo(this.markOverlays[key])
+
+ }
+ }
+
+ quaternionMult(q: number[], r: number[]) {
+ return [r[0]*q[0]-r[1]*q[1]-r[2]*q[2]-r[3]*q[3],
+ r[0]*q[1]+r[1]*q[0]-r[2]*q[3]+r[3]*q[2],
+ r[0]*q[2]+r[1]*q[3]+r[2]*q[0]-r[3]*q[1],
+ r[0]*q[3]-r[1]*q[2]+r[2]*q[1]+r[3]*q[0]]
+ }
+
+ calcOrient(w: number, x: number, y: number, z: number) {
+ // Rotation transforms from one orientation to another
+ let point = [0,0,3,0] // point vector
+ let qConj = [w,-1*x,-1*y,-1*z]
+ let q = [w,x,y,z]
+ let point_new = this.quaternionMult(this.quaternionMult(q,point),qConj)
+ return point_new.slice(1)
}
diff --git a/src/app/model/base-model.ts b/src/app/model/base-model.ts
index 33c757e9b4c81bc6726cd503d59bdd0b8fa4f025..7f07338169fc2157c757df142bcb42a0a4dad058 100644
--- a/src/app/model/base-model.ts
+++ b/src/app/model/base-model.ts
@@ -10,7 +10,7 @@ export interface Object {
type: ObjectType
sensorType: SensorType
position: Position
- orientation?: Orientation
+ orientation?: Quaternion
lastPosUpdate: string
zoneDescriptors: ZoneDesc[]
extractedAttributes?: ExtAttribute
@@ -39,7 +39,7 @@ export interface ZoneDesc {
notificationType: NotificationType
}
-interface Orientation {
+export interface Quaternion {
x: number
y: number
z: number