-
Notifications
You must be signed in to change notification settings - Fork 0
/
Program.cs
360 lines (283 loc) · 12.8 KB
/
Program.cs
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
322
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
using UIKit;
using SceneKit;
using ARKit;
using Foundation;
using System;
using CoreGraphics;
using System.Linq;
using OpenTK;
using Plugin.Geolocator;
using Plugin.Geolocator.Abstractions;
using CoreAnimation;
namespace ARKitDemo
{
public class ARDelegate : ARSCNViewDelegate
{
public override void DidAddNode(ISCNSceneRenderer renderer, SCNNode node, ARAnchor anchor)
{
if (anchor != null && anchor is ARPlaneAnchor)
{
PlaceAnchorNode(node, anchor as ARPlaneAnchor);
}
}
void PlaceAnchorNode(SCNNode node, ARPlaneAnchor anchor)
{
var plane = SCNPlane.Create(anchor.Extent.X, anchor.Extent.Z);
plane.FirstMaterial.Diffuse.Contents = UIColor.LightGray;
var planeNode = SCNNode.FromGeometry(plane);
//Locate the plane at the position of the anchor
planeNode.Position = new SCNVector3(anchor.Extent.X, 0.0f, anchor.Extent.Z);
//Rotate it to lie flat
planeNode.Transform = SCNMatrix4.CreateRotationX((float) (Math.PI / 2.0));
node.AddChildNode(planeNode);
//Mark the anchor with a small red box
var box = new SCNBox { Height = 0.1f, Width = 0.1f, Length = 0.1f };
box.FirstMaterial.Diffuse.ContentColor = UIColor.Red;
var anchorNode = new SCNNode { Position = new SCNVector3(0, 0, 0), Geometry = box };
planeNode.AddChildNode(anchorNode);
}
public override void DidUpdateNode(ISCNSceneRenderer renderer, SCNNode node, ARAnchor anchor)
{
if (anchor is ARPlaneAnchor)
{
var planeAnchor = anchor as ARPlaneAnchor;
//BUG: Extent.Z should be at least a few dozen centimeters
System.Console.WriteLine($"The (updated) extent of the anchor is [{planeAnchor.Extent.X}, {planeAnchor.Extent.Y}, {planeAnchor.Extent.Z}]");
}
}
}
public class ARKitController : UIViewController
{
ARSCNView scnView;
Position _userPosition;
public ARKitController() : base()
{
}
public override bool ShouldAutorotate() => true;
public override void ViewDidLoad()
{
base.ViewDidLoad();
scnView = new ARSCNView()
{
Frame = this.View.Frame,
Delegate = new ARDelegate(),
DebugOptions = ARSCNDebugOptions.ShowFeaturePoints | ARSCNDebugOptions.ShowWorldOrigin,
UserInteractionEnabled = true
};
this.View.AddSubview(scnView);
}
public override async void ViewWillAppear(bool animated)
{
base.ViewWillAppear(animated);
// Configure ARKit
var config = new ARWorldTrackingConfiguration();
config.WorldAlignment = ARWorldAlignment.GravityAndHeading;
//config.PlaneDetection = ARPlaneDetection.Horizontal;
// This method is called subsequent to `ViewDidLoad` so we know `scnView` is instantiated
scnView.Session.Run(config, ARSessionRunOptions.RemoveExistingAnchors);
_userPosition = await CrossGeolocator.Current.GetPositionAsync();
//////WORKS
//var markerPosition1 = new Position(59.903580, 10.776050);
//var markerPosition2 = new Position(59.903627, 10.775577);
//////
///
var markerPosition1 = new Position(59.903189, 10.775680);
var markerPosition2 = new Position(59.903429, 10.775554);
var midPoint = CalculateMidpoint(markerPosition1, markerPosition2);
var lengthTube = (float)markerPosition1.CalculateDistance(markerPosition2, GeolocatorUtils.DistanceUnits.Kilometers) * 1000;
// Create Tube
var tubeNode = PlaceTube(lengthTube);
// Move tube to correct coordinate
var transformMatrix = TransformMatrix(midPoint);
var translateVector = TranslateVector(transformMatrix);
translateVector.Y = -20;
tubeNode.Position = translateVector; //new SCNVector3(0, -20, -30);
// Angle from north axis counterclockwise and markerposition1 to markerPosition2
// ^
// m2 | Z- (North)
// \ |
// \_| <-- Bearing
//_____\| m1
var tubeBearing = CalculateBearing(markerPosition1, markerPosition2);
var origTransform = tubeNode.Transform;
// Rotate tube so it lies with length in Z-axis (North/South). Middle of tube is in origo.
//var t = tubeNode.Transform;
var newTransform = SCNMatrix4.Mult(SCNMatrix4.CreateRotationX((float)Math.PI / 2), SCNMatrix4.CreateRotationY((float)tubeBearing));
tubeNode.Transform = SCNMatrix4.Mult(newTransform, origTransform);
//tubeNode.Transform = SCNMatrix4.Mult(SCNMatrix4.CreateRotationY((float)Math.PI / 4), origTransform);//(float)tubeBearing));
//cubeNode.Position = TranslateVector(transformMatrix);
scnView.Scene.RootNode.AddChildNode(tubeNode);
//var northNode = PlaceCube(new SCNVector3(0, 0, -1));
//cubeNode.Position = TranslateVector(transformMatrix);
//cubeNode.Position = TranslateVector(transformMatrix);
}
public SCNMatrix4 AngleTube(SCNMatrix4 originalTransform)
{
var rotXTransform = SCNMatrix4.CreateRotationX((float)Math.PI / 2);
return SCNMatrix4.Mult(originalTransform, rotXTransform);
}
public SCNVector3 TranslateVector(SCNMatrix4 tMat)
{
return new SCNVector3(tMat.M14, tMat.M24, tMat.M34);
}
public SCNMatrix4 TransformMatrix(Position objectPosition)
{
// Calculates transformation matrix for object to be moved from World Coordinate System to object position
var distance = (float)_userPosition.CalculateDistance(objectPosition, GeolocatorUtils.DistanceUnits.Kilometers)*1000;
var translationMatrix = TranslationMatrix(z:distance);
var bearing = CalculateBearing(_userPosition, objectPosition)*-1;
var rotationYMatrix = RotationYMatrix(bearing);
// Same as above, but i will keep my own implementation
//var rotationYMatrix = SCNMatrix4.CreateRotationY(0.8f, out SCNMatrix4 ma);
var transformMatrix = SCNMatrix4.Mult(rotationYMatrix, translationMatrix);
var m = SCNMatrix4.Mult(SCNMatrix4.Identity, transformMatrix);
return m;
}
public Position CalculateMidpoint(Position position1, Position position2)
{
var toRadian = (Math.PI / 180);
var lat1 = position1.Latitude * toRadian;
var long1 = position1.Longitude * toRadian;
var lat2 = position2.Latitude * toRadian;
var long2 = position2.Longitude * toRadian;
var x1 = Math.Cos(lat1) * Math.Cos(long1);
var y1 = Math.Cos(lat1) * Math.Sin(long1);
var z1 = Math.Sin(lat1);
var x2 = Math.Cos(lat2) * Math.Cos(long2);
var y2 = Math.Cos(lat2) * Math.Sin(long2);
var z2 = Math.Sin(lat2);
var X = (x1 + x2) / 2;
var Y = (y1 + y2) / 2;
var Z = (z1 + z2) / 2;
var lon = Math.Atan2(Y, X);
var hyp = Math.Sqrt(X * X + Y * Y);
var lat = Math.Atan2(Z, hyp);
var toCartesian = (180 / Math.PI);
var midPosition = new Position(lat * toCartesian, lon * toCartesian);
return midPosition;
}
public double CalculateBearing(Position position1, Position position2)
{
// Calculates angle from north-axis (-Z) with position1 as origo to position2
var toRadian = (Math.PI / 180);
var lat1 = position1.Latitude * toRadian;
var long1 = position1.Longitude * toRadian;
var lat2 = position2.Latitude * toRadian;
var long2 = position2.Longitude * toRadian;
var deltaLong = long2 - long1;
var yCoord = Math.Cos(lat2) * Math.Sin(deltaLong);
var xCoord = Math.Cos(lat1)*Math.Sin(lat2) -
Math.Sin(lat1)*Math.Cos(lat2)*Math.Cos(deltaLong);
var bearing = Math.Atan2(yCoord, xCoord);
return bearing;
}
public SCNMatrix4 TranslationMatrix(float x = 0, float y = 0, float z = 0)
{
var translationMatrix = new SCNMatrix4(new SCNVector4(1, 0, 0, -x),
new SCNVector4(0, 1, 0, -y),
new SCNVector4(0, 0, 1, -z),
new SCNVector4(0, 0, 0, 1));
return translationMatrix;
}
public SCNMatrix4 RotationYMatrix(double bearing)
{
var rotationMatrix = SCNMatrix4.Identity;
rotationMatrix.M11 = (float)Math.Cos(bearing);
rotationMatrix.M13 = -(float)Math.Sin(bearing);
rotationMatrix.M31 = (float)Math.Sin(bearing);
rotationMatrix.M33 = (float)Math.Cos(bearing);
var invertedRotationMatrix = SCNMatrix4.Invert(rotationMatrix);
return invertedRotationMatrix;
}
public override void TouchesBegan(NSSet touches, UIEvent evt)
{
base.TouchesBegan(touches, evt);
var touch = touches.AnyObject as UITouch;
if (touch != null)
{
var loc = touch.LocationInView(scnView);
var worldPos = WorldPositionFromHitTest(loc);
if (worldPos.Item1.HasValue)
{
PlaceCube(worldPos.Item1.Value);
}
}
}
private SCNVector3 PositionFromTransform(NMatrix4 xform)
{
return new SCNVector3(xform.M14, xform.M24, xform.M34);
}
Tuple<SCNVector3?, ARAnchor> WorldPositionFromHitTest (CGPoint pt)
{
//Hit test against existing anchors
var hits = scnView.HitTest(pt, ARHitTestResultType.ExistingPlaneUsingExtent);
if (hits != null && hits.Length > 0)
{
var anchors = hits.Where(r => r.Anchor is ARPlaneAnchor);
if (anchors.Count() > 0)
{
var first = anchors.First();
var pos = PositionFromTransform(first.WorldTransform);
return new Tuple<SCNVector3?, ARAnchor>(pos, (ARPlaneAnchor)first.Anchor);
}
}
return new Tuple<SCNVector3?, ARAnchor>(null, null);
}
private SCNMaterial[] LoadMaterials()
{
Func<string, SCNMaterial> LoadMaterial = fname =>
{
var mat = new SCNMaterial();
mat.Diffuse.Contents = UIImage.FromFile(fname);
mat.LocksAmbientWithDiffuse = true;
return mat;
};
var a = LoadMaterial("msft_logo.png");
var b = LoadMaterial("xamagon.png");
var c = LoadMaterial("fsharp.png"); // This demo was originally in F# :-)
return new[] { a, b, a, b, c, c };
}
SCNNode PlaceCube(SCNVector3 pos)
{
var box = new SCNBox { Width = 0.25f, Height = 0.25f, Length = 0.25f };
var cubeNode = new SCNNode { Position = pos, Geometry = box};
cubeNode.Geometry.Materials = LoadMaterials();
scnView.Scene.RootNode.AddChildNode(cubeNode);
return cubeNode;
}
SCNNode PlaceCube()
{
var box = new SCNBox { Width = 0.25f, Height = 0.25f, Length = 0.25f };
var cubeNode = new SCNNode { Geometry = box };
cubeNode.Geometry.Materials = LoadMaterials();
return cubeNode;
}
SCNNode PlaceTube(float height)
{
var tube = new SCNTube { Height = height, OuterRadius = 0.2f, InnerRadius = 0.1f};
var tubeNode = new SCNNode { Geometry = tube };
//cubeNode.Geometry.Materials = LoadMaterials();
return tubeNode;
}
}
[Register ("AppDelegate")]
public class AppDelegate : UIApplicationDelegate
{
UIWindow window;
ARKitController viewController;
public override bool FinishedLaunching (UIApplication app, NSDictionary options)
{
window = new UIWindow (UIScreen.MainScreen.Bounds);
window.RootViewController = new ARKitController();
window.MakeKeyAndVisible ();
return true;
}
}
public class Application
{
static void Main (string [] args)
{
UIApplication.Main (args, null, "AppDelegate");
}
}
}