Create Quaternion from Acceleration Data - c#

So I have an acceleration sensor that gives me acceleration data. The device is currently resting at a certain position, so the data looks like this (with some noise per axis):
ACCELX = 264
ACCELY = -43
ACCELZ = 964
Then there's a 3D model represting the device, and "all I want" is this 3D model to represent the real device's orientation. In my attempts to understand the usage of quaternions in .NET, here's the code I've gobbled up:
/* globals */
Vector3D PrevVector = new Vector3D(0, 0, 0);
ModelVisual3D model; // initialized and model file loaded
private async void TimerEvent()
{
RotateTransform3D rot = new RotateTransform3D();
QuaternionRotation3D q = new QuaternionRotation3D();
double x = 0, y = 0, z = 0;
List<Reading> results = await Device.ReadSensor();
foreach (Reading r in results)
{
switch (r.Type)
{
case "RPF_SEN_ACCELX":
x = r.Value;
break;
case "RPF_SEN_ACCELY":
y = r.Value;
break;
case "RPF_SEN_ACCELZ":
z = r.Value;
break;
}
}
double angle = Vector3D.AngleBetween(new Vector3D(x, y, z), PrevVector);
q.Quaternion = new Quaternion(new Vector3D(x, y, z), angle);
rot.Rotation = q;
model.Transform = rot;
PrevVector = new Vector3D(x, y, z);
}
Moving my real device does yield changes in the reported values, but the model on the screen just twitches in what seems to me random directions, little more far than from the noise and seemingly unrelated to how I rotate the real device. I'm fairly sure I'm constructing and using quaternions incorrectly. How would I do it right?
This is .NET with WPF. There's also HelixToolkit.WPF available, but I haven't seen any function to create quaternions from acceleration data in there. Higher level frameworks such as Unreal Engine or Unity are NOT available for this project.

Is your sensor output rotation value accumulative or differences? Sometimes the output rotation is differences, and you need previous rotation value plus the differences to calculate the current new one.
You can try to save the previous quaternion and add current quaterion with previous quaternion to get the new accumulated rotation.

Turns out my issue was of a completely different nature: I had to utilize a class called Transform3DGroup. This is how the code must be altered to enable a rotation around the Z axis:
/* globals */
ModelVisual3D model; // initialized and model file loaded
Transform3DGroup tg = new Transform3DGroup();
private async void TimerEvent()
{
RotateTransform3D rot = new RotateTransform3D();
QuaternionRotation3D q = new QuaternionRotation3D();
double x = 0, y = 0, z = 0;
List<Reading> results = await Device.ReadSensor();
foreach (Reading r in results)
{
switch (r.Type)
{
case "RPF_SEN_ACCELX":
x = r.Value;
break;
case "RPF_SEN_ACCELY":
y = r.Value;
break;
case "RPF_SEN_ACCELZ":
z = r.Value;
angle = GetAngle(x, y).ToDegrees();
q.Quaternion = new Quaternion(new Vector3D(0, 0, 1), angle);
break;
}
rot.Rotation = q;
tg.Children.Clear();
tg.Children.Add(rot);
model.Transform = tg; // Use Transform3DGroup!
}
}
I haven't found any documentation on the obligatory use of Transform3DGroup.
For the sake of completion, here are the internals of GetAngle(), as derived by Bill Wallis on Math Overflow:
private double GetAngle(double x, double y)
{
if (x > 0)
{
return 2 * Math.PI - Math.Atan(y / x);
}
else if (x < 0)
{
return Math.PI - Math.Atan(y / x);
}
else // x == 0
{
return 2 * Math.PI - Math.Sign(y) * Math.PI / 2;
}
}
And the extensions to the double type to transform doubles between radians and degrees (outside of class, within namespace):
public static class NumericExtensions
{
public static double ToRadians(this double val)
{
return (Math.PI / 180) * val;
}
public static double ToDegrees(this double val)
{
return (180 / Math.PI) * val;
}
}

Related

Why Radial tree layout drawing algorithm is making crossed edges?

I am implementing radial layout drawing algorithm, according to the publication of mr.Andy Pavlo link [page 18]
The problem is, that my result contains crossed edges. Which is something that is unacceptable. I found some solution, similiar problem link but I was not able to implement them into this algorithm (I would have to change the whole approach to the solution). In addition, the algorithm by Mr. Andy Pavlo should be able to solve this problem. When we look at the result of its algorithm, there are no crossed edges here. What am I doing wrong? Am I missing something? Thank you in advance.
Mr.Pavlo pseudo code of algorithm
My implementation of algorithm
public void RadialPositions(Tree<string> rootedTree, Node<string> vertex, double alfa, double beta,
List<RadialPoint<string>> outputGraph)
{
//check if vertex is root of rootedTree
if (vertex.IsRoot)
{
vertex.Point.X = 0;
vertex.Point.Y = 0;
outputGraph.Add(new RadialPoint<string>
{
Node = vertex,
Point = new Point
{
X = 0,
Y = 0
},
ParentPoint = null
});
}
//Depth of vertex starting from 0
int depthOfVertex = vertex.Depth;
double theta = alfa;
double radius = Constants.CircleRadius + (Constants.Delta * depthOfVertex);
//Leaves number in the subtree rooted at v
int leavesNumber = BFS.BreatFirstSearch(vertex);
foreach (var child in vertex.Children)
{
//Leaves number in the subtree rooted at child
int lambda = BFS.BreatFirstSearch(child);
double mi = theta + ((double)lambda / leavesNumber * (beta - alfa));
double x = radius * Math.Cos((theta + mi) / 2.0);
double y = radius * Math.Sin((theta + mi) / 2.0);
//setting x and y
child.Point.X = x;
child.Point.Y = y;
outputGraph.Add(new RadialPoint<string>
{
Node = child,
Point = new Point
{
X = x,
Y = y,
Radius = radius
},
ParentPoint = vertex.Point
});
if (child.Children.Count > 0)
{
child.Point.Y = y;
child.Point.X = x;
RadialPositions(rootedTree, child, theta, mi, outputGraph);
}
theta = mi;
}
}
BFS algorithm for getting leaves
public static int BreatFirstSearch<T>(Node<T> root)
{
var visited = new List<Node<T>>();
var queue = new Queue<Node<T>>();
int leaves = 0;
visited.Add(root);
queue.Enqueue(root);
while (queue.Count != 0)
{
var current = queue.Dequeue();
if (current.Children.Count == 0)
leaves++;
foreach (var node in current.Children)
{
if (!visited.Contains(node))
{
visited.Add(node);
queue.Enqueue(node);
}
}
}
return leaves;
}
Initial call
var outputPoints = new List<RadialPoint<string>>();
alg.RadialPositions(tree, tree.Root,0, 360, outputPoints);
mr.Pavlo result
My result on simple sample
Math.Cos and Sin expect the input angle to be in radians, not degrees. In your initial method call, your upper angle limit (beta) should be 2 * Math.PI, not 360. This will ensure that all the angles you calculate will be in radians and not degrees.

Considering only 3D points, wouldn't this naive approach to the "smallest enclosing ball" be good enough?

Lets use C# in our example.
public class Sphere
{
public Point Center { get; set; }
public float Radius { get; set; }
public Sphere(IEnumerable<Point> points)
{
Point first = points.First();
Point vecMaxZ = first;
Point vecMinZ = first;
Point vecMaxY = first;
Point vecMinY = first;
Point vecMinX = first;
Point vecMaxX = first;
foreach (Point current in points)
{
if (current.X < vecMinX.X)
{
vecMinX = current;
}
if (current.X > vecMaxX.X)
{
vecMaxX = current;
}
if (current.Y < vecMinY.Y)
{
vecMinY = current;
}
if (current.Y > vecMaxY.Y)
{
vecMaxY = current;
}
if (current.Z < vecMinZ.Z)
{
vecMinZ = current;
}
if (current.Z > vecMaxZ.Z)
{
vecMaxZ = current;
}
}
//the lines bellow assure at least 2 points sit on the surface of the sphere.
//I'm pretty sure the algorithm is solid so far, unless I messed up the if/elses.
//I've been over this, looking at the variables and the if/elses and they all
//seem correct, but our own errors are the hardest to spot,
//so maybe there's something wrong here.
float diameterCandidateX = vecMinX.Distance(vecMaxX);
float diameterCandidateY = vecMinY.Distance(vecMaxY);
float diameterCandidateZ = vecMinZ.Distance(vecMaxZ);
Point c;
float r;
if (diameterCandidateX > diameterCandidateY)
{
if (diameterCandidateX > diameterCandidateZ)
{
c = vecMinX.Midpoint(vecMaxX);
r = diameterCandidateX / 2f;
}
else
{
c = vecMinZ.Midpoint(vecMaxZ);
r = diameterCandidateZ / 2f;
}
}
else if (diameterCandidateY > diameterCandidateZ)
{
c = vecMinY.Midpoint(vecMaxY);
r = diameterCandidateY / 2f;
}
else
{
c = vecMinZ.Midpoint(vecMaxZ);
r = diameterCandidateZ / 2f;
}
//the lines bellow look for points outside the sphere, and if one is found, then:
//1 let dist be the distance from the stray point to the current center
//2 let diff be the equal to dist - radius
//3 radius will then the increased by half of diff.
//4 a vector with the same direction as the stray point but with magnitude equal to diff is found
//5 the current center is moved by half the vector found in the step above.
//
//the stray point will now be included
//and, I would expect, the relationship between the center and other points will be mantained:
//if distance from p to center = r / k,
//then new distance from p to center' = r' / k,
//where k doesn't change from one equation to the other.
//this is where I'm wrong. I cannot figure out how to mantain this relationship.
//clearly, I'm moving the center by the wrong amount, and increasing the radius wrongly too.
//I've been over this problem for so much time, I cannot think outside the box.
//my whole world is the box. The box and I are one.
//maybe someone from outside my world (the box) could tell me where my math is wrong, please.
foreach (Point current in points)
{
float dist = current.Distance(c);
if (dist > r)
{
float diff = dist - r;
r += diff / 2f;
float scaleFactor = diff / current.Length();
Point adjust = current * scaleFactor;
c += adjust / 2f;
}
}
Center = c;
Radius = r;
}
public bool Contains(Point point) => Center.Distance(point) <= Radius;
public override string ToString() => $"Center: {Center}; Radius: {Radius}";
}
public class Point
{
public float X { get; set; }
public float Y { get; set; }
public float Z { get; set; }
public Point(float x, float y, float z)
{
X = x;
Y = y;
Z = z;
}
public float LengthSquared() => X * X + Y * Y + Z * Z;
public float Length() => (float) Math.Sqrt(X * X + Y * Y + Z * Z);
public float Distance(Point another)
{
return (float) Math.Sqrt(
(X - another.X) * (X - another.X)
+ (Y - another.Y) * (Y - another.Y)
+ (Z - another.Z) * (Z - another.Z));
}
public float DistanceSquared(Point another)
{
return (X - another.X) * (X - another.X)
+ (Y - another.Y) * (Y - another.Y)
+ (Z - another.Z) * (Z - another.Z);
}
public Point Perpendicular()
{
return new Point(-Y, X, Z);
}
public Point Midpoint(Point another)
{
return new Point(
(X + another.X) / 2f,
(Y + another.Y) / 2f,
(Z + another.Z) / 2f);
}
public override string ToString() => $"({X}, {Y}, {Z})";
public static Point operator +(Point p1, Point p2)
{
return new Point(p1.X + p2.X, p1.Y + p2.Y, p1.Z + p2.Z);
}
public static Point operator *(Point p1, float v)
{
return new Point(p1.X * v, p1.Y * v, p1.Z * v);
}
public static Point operator /(Point p1, float v)
{
return new Point(p1.X / v, p1.Y / v, p1.Z / v);
}
}
//Note: this class is here so I can be able to solve the problems suggested by
//Eric Lippert.
public class Line
{
private float coefficient;
private float constant;
public Line(Point p1, Point p2)
{
float deltaY = p2.Y - p1.Y;
float deltaX = p2.X - p1.X;
coefficient = deltaY / deltaX;
constant = coefficient * -p1.X + p1.Y;
}
public Point FromX(float x)
{
return new Point(x, x * coefficient + constant, 0);
}
public Point FromY(float y)
{
return new Point((y - constant) / coefficient, y, 0);
}
public Point Intersection(Line another)
{
float x = (another.constant - constant) / (coefficient - another.coefficient);
float y = FromX(x).Y;
return new Point(x, y, 0);
}
}
Can I safely assume this will run at least just as fast as the fancy algorithms out there that usually consider, for robustness sake, the possibility of the Points having any number of dimensions, from 2 to anything, like 1000 or 10,000 dimensions.
I only need it for 3 dimensions, never more and never less than that. Since I have no academic degree on computer science (or any degree for that matter, I'm a highschool sophomore), I have difficulties in analyzing algorithms for performance and resource consumption. So my question basically is: Is my "smallest enclosing sphere for dumbs" algoritm good in performance and resource consumption when compared with the fancy ones? Is there a point where my algorithm breaks while the professional ones don't, meaning it performs so bad it will cause noticeable loss (like, if I have too many points).
EDIT 1: I editted the code because it made no sense at all (I was hungry, it was 4pm and I haven't eaten all day). This one makes more sense I think, not sure if it's correct though. The original question stands: If this one solves the problem, does it do it well enough to compete with the stardard professional algorithms in case we know in advance that all points have 3 dimensions?
EDIT 2: Now I'm pretty sure the performance is bad, and I lost all hope of implementing a naive algorithm to find the smallest enclosing sphere. I just want to make something that work. Please, check the latest update.
EDIT 3: Doesn't work either. I quit.
EDIT 4: Finally, after, I don't know... some 5 hours. I figured it out. Jesus Christ. This one works. Could someone tell me about the performance issue? Is it really bad compared to the professional algorithms? What lines can I change to make it better? Is there a point where it breaks? Remember, I will always use it for 3D points.
EDIT 5: I learned from Bychenko the previous algorithm still didn't work. I slept on this issue, and this is my new version of the algorithm. I know it doesn't work, and I have a good clue where it is wrong, could anyone please tell why those particular calculations are wrong and how to fix them? I'm inclined to think this has something to do with trigonometry. My assumptions don't hold true for Euclidean space, because I can't stop seeing vectors as real numbers instead
of sets of real numbers that, in my case, I use to pin-point a location in Euclidean space. I'm pretty sure I'm missing some sine or cosine somewhere in the last loop (of course, not exactly sine or cosine, but the equivalent in cartesian coordinates, since we don't know any angles.
Addendum to EDIT 5: About the problems proposed by Eric Lippert:
(1) argh too trivial :p
(2) I will do it for the circle first; I will add a class Line for that.
Point a, b, c; //they are not collinear
Point midOfAB = a.Midpoint(b);
Point midOfBC = b.Midpoint(c);
//multiplying the vector by a scalar as I do bellow doesn't matter right?
Point perpendicularToAB = midOfAB.Perpendicular() * 3;
Point perpendicularToBC = midOfBC.Perpendicular() * 3;
Line bisectorAB = new Line(perpendicularToAB, midOfAB);
Line bisectorBC = new Line(perpendicularToBC, midOfBC);
Point center = bisectorAB.Intersection(bisectorBC);
float distA = center.Distance(a);
float distB = center.Distance(b);
float distC = center.Distance(c);
if(distA == distB && distB == distC)
//it works (spoiler alert: it doesn't)
else
//you're a failure, programmer, pick up your skate and practice some ollies
Sorry, but your algorithm is wrong. It doesn't solve the problem.
Counter example (3 points):
A = (0, 0, 0) - closest to origin (0)
B = (3, 3, 0) - farthest from origin (3 * sqrt(2) == 4.2426...)
C = (4, 0, 0)
your naive algorithm declares that the sphere has center at
P = (3 / sqrt(2), 3 / sqrt(2), 0)
and radius
R = 3 / sqrt(2)
and you can see that the point C = (4, 0, 0) is beyond the sphere
Edit the updated (but naive) algorithm is still wrong.
Counter example (3 points):
A = (0, 0, 0)
B = (1, 2, 0)
C = (4, 1, 0)
according the algorithm the sphere has its center at
P = (2, 1, 0)
with radius
R = sqrt(5)
and you can see that the sphere is not a minimal (smallest) one.
Nth Edit you still have an incorrect algorithm. When exploring gray zone (you know the problem, but partially, with holes) it's a good practice to invest into testing automatition. As you should know, in case of triangle all the vertexes should be on the sphere; let's validate your the solution on this fact:
public static class SphereValidator {
private static Random m_Random = new Random();
private static String Validate() {
var triangle = Enumerable
.Range(0, 3)
.Select(i => new Point(m_Random.Next(100), m_Random.Next(100), m_Random.Next(100)))
.ToArray();
Sphere solution = new Sphere(triangle);
double tolerance = 1.0e-5;
for (int i = 0; i < triangle.Length; ++i) {
double r = triangle[i].Distance(solution.Center);
if (Math.Abs(r - solution.Radius) > tolerance) {
return String.Format("Counter example\r\n A: {0}\r\n B: {1}\r\n C: {2}\r\n expected distance to \"{3}\": {4}; actual R {5}",
triangle[0], triangle[1], triangle[2], (char) ('A' + i), r, solution.Radius);
}
}
return null;
}
public static String FindCounterExample(int attempts = 10000) {
for (int i = 0; i < attempts; ++i) {
String result = Validate();
if (!String.IsNullOrEmpty(result))
Console.WriteLine(result);
return;
}
Console.WriteLine(String.Format("Yes! All {0} tests passed!", attempts));
}
}
I've just run the code above and got:
Counter example
A: (3, 30, 9)
B: (1, 63, 40)
C: (69, 1, 16)
expected distance to "A": 35.120849609375; actual R 53.62698
For a crude approximation, compute the Axis-Aligned Bounding Box, then the bounding sphere of that box (same center, diameter = √(W² + H² + D²) ).
You can refine by computing the largest distance from that center.

Point-Zoom on Mandelbrot Set in C# - It works, except when the mouse has moved

I'm able to point zoom on the Mandelbrot set, as long as the mouse doesn't move after zooming has begun. I've tried calculating a normalized delta (new coordinate - old coordinate)*(oldzoom), but what happens is the image appears to jump around to a new location. I've seen this issue before. I'm struggling more here because I have to somehow convert this mouse position delta back to the -2,2 coordinate space of the Mandelbrot set.
Here's my code. What's important is the GetZoomPoint method, and then the lines of code that define x0 and y0. Also, I use the Range class to scale values from one range to another. I WAS using deltaTrans (thats the thing I was talking about earlier where I normalize the mouse delta with the old scale).
using OpenTK.Graphics.OpenGL;
using SpriteSheetMaker;
using System;
using System.Collections.Generic;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
namespace Fractal.Fractal
{
public class Mandelbrot : BaseTexture
{
private static Transform GlobalTransform = SpriteSheetMaker.Global.Transform;
private static Vector3 GlobalScale = GlobalTransform.Scale;
private static Vector3 GlobalTrans = GlobalTransform.Translation;
private static Vector3 LastWindowPoint = null;
private static Vector3 ZoomFactor = Vector3.ONE * 1.2f;
private static Vector3 Displacement = Vector3.ZERO;
private static int WindowSize = 100;
public static Vector3 GetZoomPoint()
{
var zP = OpenGLHelpers.LastZoomPoint.Clone();
if (LastWindowPoint == null)
{
LastWindowPoint = zP.Clone();
}
var delta = zP - LastWindowPoint;
var oldZoom = GlobalScale / ZoomFactor;
var deltaTrans = delta.XY * oldZoom.XY;
var factor = ZoomFactor.Clone();
Range xR = new Range(0, WindowSize);
Range yR = new Range(0, WindowSize);
Range complexRange = new Range(-2, 2);
// Calculate displacement of zooming position.
var dx = (zP.X - Displacement.X) * (factor.X - 1f);
var dy = (zP.Y - Displacement.Y) * (factor.Y - 1f);
// Compensate for displacement.
Displacement.X -= dx;
Displacement.Y -= dy;
zP -= Displacement;
var x = complexRange.ScaleValue(zP.X, xR);
var y = complexRange.ScaleValue(zP.Y, yR);
var rtn = new Vector3(x, y);
LastWindowPoint = zP.Clone();
return rtn;
}
public static Mandelbrot Generate()
{
var size = new Size(WindowSize, WindowSize);
var radius = new Size(size.Width / 2, size.Height / 2);
Bitmap bmp = new Bitmap(size.Width, size.Height);
LockBitmap.LockBitmapUnsafe lbm = new LockBitmap.LockBitmapUnsafe(bmp);
lbm.LockBits();
var pt = Mandelbrot.GetZoomPoint();
Parallel.For(0, size.Width, i =>
{
// float x0 = complexRangeX.ScaleValue(i, xRange);
float x0 = ((i - radius.Width) / GlobalScale.X) + pt.X;
Parallel.For(0, size.Height, j =>
{
// float y0 = complexRangeY.ScaleValue(j, yRange);
float y0 = ((j - radius.Height) / GlobalScale.Y) + pt.Y;
float value = 0f;
float x = 0.0f;
float y = 0.0f;
int iteration = 0;
int max_iteration = 100;
while (x * x + y * y <= 4.0 && iteration < max_iteration)
{
float xtemp = x * x - y * y + x0;
y = 2.0f * x * y + y0;
x = xtemp;
iteration += 1;
if (iteration == max_iteration)
{
value = 255;
break;
}
else
{
value = iteration * 50f % 255f;
}
}
int v = (int)value;
lbm.SetPixel(i, j, new ColorLibrary.HSL(v / 255f, 1.0, 0.5).ToDotNetColor());
});
});
lbm.UnlockBits();
var tex = new BaseTextureImage(bmp);
var rtn = new Mandelbrot(tex);
return rtn;
}
public override void Draw()
{
base._draw();
}
private Mandelbrot(BaseTextureImage graphic)
{
var topLeft = new Vector3(0, 1);
var bottomLeft = new Vector3(0, 0);
var bottomRight = new Vector3(1, 0);
var topRight = new Vector3(1, 1);
this.Vertices = new List<Vector3>()
{
topLeft,bottomLeft,bottomRight,topRight
};
this.Size.X = WindowSize;
this.Size.Y = WindowSize;
this.Texture2D = graphic;
}
}
}
I refactored my code, and also figured out a solution to this problem. 2 big wins in one. Ok, so I found a solution on CodeProject written in C# which I was readily able to adapt to my project. I'm not sure why I didn't realize this when I posted the question, but what I needed to solve this issue was to create a 'window' of zoom and not think in terms of a 'point zoom'. Yes, even if I am trying to zoom directly into a point, that point is just the center of some sort of a window.
Here is the method I have, which expects start and end mousedown coordinates (screen space), and converts the mandelbrot set window size accordingly.
public void ApplyZoom(double x0, double y0, double x1, double y1)
{
if (x1 == x0 && y0 == y1)
{
//This was just a click, no movement occurred
return;
}
/*
* XMin, YMin and XMax, YMax are the current extent of the set
* mx0,my0 and mx1,my1 are the part we selected
* do the math to draw the selected rectangle
* */
double scaleX, scaleY;
scaleX = (XMax - XMin) / (float)BitmapSize;
scaleY = (YMax - YMin) / (float)BitmapSize;
XMax = (float)x1 * scaleX + XMin;
YMax = (float)y1 * scaleY + YMin;
XMin = (float)x0 * scaleX + XMin;
YMin = (float)y0 * scaleY + YMin;
this.Refresh(); // force mandelbrot to redraw
}
Basically, whats happening is we calculate the ratio between the mandelbrot window size versus the screen size we are drawing to. Then, using that scale, we basically convert our mousedown coordinates to mandelbrot set coordinates (x1*scaleX, etc) and manipulate the current Min and Max coordinates with them, using the Min values as the pivot point.
Here's the link to the CodeProject I used as a reference: CodeProject link

Calculating the Area of a Closed Polygon on a Plane

I'm attempting to calculate the area of a polygon that lies on a plane (a collection co-planar points forming a non-intersecting closed shape), and I know a method that can calculate the area of an irregular (or any) polygon in two dimensions - but not three. My solution is to rotate the plane so that it's normal is 0 in the z direction (so I can treat it like it's 2D) and then run the 2D area function.
The problem is I have NO idea how to actually determine the rotation axes and amounts to flatten a plane on it's Z-axis. I do my rotation through the easiest method I could find for 3 dimensional rotation: Rotation Matrices. So, given that I'm trying to use rotation matrices to do my rotation, how do I figure out the angles to rotate my plane by to be oriented in the same direction as another vector? I don't actually know much calculus or Euclidean geometry, so whichever solution requires me to teach myself the least of both is the ideal solution. Is there a better way?
Here's my attempt below, which doesn't even come close to getting the plane flat on the Z axis. This is an instance method of my "Surface" class, which is a derivative of my "Plane" class, and has an array of co-planar points (IntersectPoints) forming a closed polygon.
public virtual double GetArea()
{
Vector zUnit = new Vector(0, 0, 1); //vector perprendicualr to z
Vector nUnit = _normal.AsUnitVector();
Surface tempSurface = null;
double result = 0;
if (nUnit != zUnit && zUnit.Dot(nUnit) != 0) //0 = perprendicular to z
{
tempSurface = (Surface)Clone();
double xAxisAngle = Vector.GetAxisAngle(nUnit, zUnit, Physics.Formulae.Axes.X);
double yAxisAngle = Vector.GetAxisAngle(nUnit, zUnit, Physics.Formulae.Axes.Y);
double rotationAngle = Vector.GetAxisAngle(nUnit, zUnit, Physics.Formulae.Axes.Z);
tempSurface.Rotate(xAxisAngle, yAxisAngle, rotationAngle); //rotating plane so that it is flat on the Z axis
}
else
{
tempSurface = this;
}
for (int x = 0; x < tempSurface.IntersectPoints.Count; x++) //doing a cross sum of each point
{
Point curPoint = tempSurface.IntersectPoints[x];
Point nextPoint;
if (x == tempSurface.IntersectPoints.Count - 1)
{
nextPoint = tempSurface.IntersectPoints[0];
}
else
{
nextPoint = tempSurface.IntersectPoints[x + 1];
}
double cross1 = curPoint.X * nextPoint.Y;
double cross2 = curPoint.Y * nextPoint.X;
result += (cross1 - cross2); //add the cross sum of each set of points to the result
}
return Math.Abs(result / 2); //divide cross sum by 2 and take its absolute value to get the area.
}
And here are my core rotation and get axis angle methods:
private Vector Rotate(double degrees, int axis)
{
if (degrees <= 0) return this;
if (axis < 0 || axis > 2) return this;
degrees = degrees * (Math.PI / 180); //convert to radians
double sin = Math.Sin(degrees);
double cos = Math.Cos(degrees);
double[][] matrix = new double[3][];
//normalizing really small numbers to actually be zero
if (Math.Abs(sin) < 0.00000001)
{
sin = 0;
}
if (Math.Abs(cos) < 0.0000001)
{
cos = 0;
}
//getting our rotation matrix
switch (axis)
{
case 0: //x axis
matrix = new double[][]
{
new double[] {1, 0, 0},
new double[] {0, cos, sin * -1},
new double[] {0, sin, cos}
};
break;
case 1: //y axis
matrix = new double[][]
{
new double[] {cos, 0, sin},
new double[] {0, 1, 0},
new double[] {sin * -1, 0, cos}
};
break;
case 2: //z axis
matrix = new double[][]
{
new double[] {cos, sin * -1, 0},
new double[] {sin, cos, 0},
new double[] {0, 0, 1}
};
break;
default:
return this;
}
return Physics.Formulae.Matrix.MatrixByVector(this, matrix);
}
public static double GetAxisAngle(Point a, Point b, Axes axis, bool inDegrees = true)
{ //pretty sure this doesnt actually work
double distance = GetDistance(a, b);
double difference;
switch (axis)
{
case Axes.X:
difference = b.X - a.X;
break;
case Axes.Y:
difference = b.Y - a.Y;
break;
case Axes.Z :
difference = b.Z - a.Z;
break;
default:
difference = 0;
break;
}
double result = Math.Acos(difference / distance);
if (inDegrees == true)
{
return result * 57.2957; //57.2957 degrees = 1 radian
}
else
{
return result;
}
}
A robust way to do this is to do a sum of the cross-products of the vertices of each edge. If your vertices are co-planar, this will produce a normal to the plane, whose length is 2 times the area of the closed polygon.
Note that this method is very similar to the 2D method linked in your question, which actually calculates a 2D equivalent of the 3D cross-product, summed for all edges, then divides by 2.
Vector normal = points[count-1].cross(points[0]);
for(int i=1; i<count; ++i) {
normal += points[i-1].cross(points[i]);
}
double area = normal.length() * 0.5;
Advantages of this method:
If your vertices are only approximately planar, it still gives the right answer
It doesn't depend on the angle of the plane.
In fact you don't need to deal with the angle at all.
If you want to know the plane orientation, you've got the normal already.
One possible difficulty: if your polygon is very small, and a long way away from the origin, you can get floating point precision problems. If that case is likely to arise, you should first translate all of your vertices so that one is at the origin, like so:
Vector normal(0,0,0);
Vector origin = points[count-1];
for(int i=1; i<count-1; ++i) {
normal += (points[i-1]-origin).cross(points[i]-origin);
}
double area = normal.length() * 0.5;
You need not to rotate the plane (or all points). Just calculate an area of polygon projection to Z-plane (if it is not perpendicular to polygon plane), for example, with you GetArea function, and divide result by cosinus of Poly-plane - Z-plane angle - it is equal to scalar product of zUnit and nUnit (I suggest that nUnit is normal vector to polygon plane)
TrueArea = GetArea() / zUnit.Dot(nUnit)

C# GMap.Net calculate surface of polygon

I am searching for a way to calculate the surface under a polygon.
The thing I want to accomplish is that a user that uses my program, can create a polygon to mark out his property. Now I want to know what the surface area is so I can tell the user how big his property is.
Unit m² or km² or hectare.
The points of the polygon have a latitude and longitude.
I am using C# with WPF and GMap.NET. The map is in a WindowsFormHost so I can use the Winforms thing from GMap.Net because this provoides overlays etc.
I hope that someone can help me or show me a post where this is explained that I didn't found.
Using a 2D vector space approximation (local tangent space)
In this section, I can detail how I come to these formulas.
Let's note Points the points of the polygon (where Points[0] == Points[Points.Count - 1] to close the polygon).
The idea behind the next methods is to split the polygon into triangles (the area is the sum of all triangle areas). But, to support all polygon types with a simple decomposition (not only star-shaped polygon), some triangle contributions are negative (we have a "negative" area). The triangles decomposition I use is : {(O, Points[i], Points[i + 1]} where O is the origin of the affine space.
The area of a non-self-intersecting polygon (in euclidian geometry) is given by:
In 2D:
float GetArea(List<Vector2> points)
{
float area2 = 0;
for (int numPoint = 0; numPoint < points.Count - 1; numPoint++)
{
MyPoint point = points[numPoint];
MyPoint nextPoint = points[numPoint + 1];
area2 += point.x * nextPoint.y - point.y * nextPoint.x;
}
return area2 / 2f;
}
In 3D, given normal, the unitary normal of the polygon (which is planar):
float GetArea(List<Vector3> points, Vector3 normal)
{
Vector3 vector = Vector3.Zero;
for (int numPoint = 0; numPoint < points.Count - 1; numPoint++)
{
MyPoint point = points[numPoint];
MyPoint nextPoint = points[numPoint + 1];
vector += Vector3.CrossProduct(point, nextPoint);
}
return (1f / 2f) * Math.Abs(Vector3.DotProduct(vector, normal));
}
In the previous code I assumed you have a Vector3 struct with Add, Subtract, Multiply, CrossProduct and DotProduct operations.
In your case, you have a lattitude and longitude. Then, you are not in an 2D euclidean space. It is a spheric space where computing the area of any polygon is much more complex.
However, it is locally homeomorphic to a 2D vector space (using the tangent space).
Then, if the area you try to measure is not too wide (few kilometers), the above formula should work.
Now, you just have to find the normal of the polygon. To do so, and to reduce the error (because we are approximating the area), we use the normal at the centroid of the polygon. The centroid is given by:
Vector3 GetCentroid(List<Vector3> points)
{
Vector3 vector = Vector3.Zero;
Vector3 normal = Vector3.CrossProduct(points[0], points[1]); // Gets the normal of the first triangle (it is used to know if the contribution of the triangle is positive or negative)
normal = (1f / normal.Length) * normal; // Makes the vector unitary
float sumProjectedAreas = 0;
for (int numPoint = 0; numPoint < points.Count - 1; numPoint++)
{
MyPoint point = points[numPoint];
MyPoint nextPoint = points[numPoint + 1];
float triangleProjectedArea = Vector3.DotProduct(Vector3.CrossProduct(point, nextPoint), normal);
sumProjectedAreas += triangleProjectedArea;
vector += triangleProjectedArea * (point + nextPoint);
}
return (1f / (6f * sumProjectedAreas)) * vector;
}
I've added a new property to Vector3 : Vector3.Length
Finally, to convert latitude and longitude into a Vector3:
Vector3 GeographicCoordinatesToPoint(float latitude, float longitude)
{
return EarthRadius * new Vector3(Math.Cos(latitude) * Math.Cos(longitude), Math.Cos(latitude) * Math.Sin(longitude), Math.Sin(latitude));
}
To sum up:
// Converts the latitude/longitude coordinates to 3D coordinates
List<Vector3> pointsIn3D = (from point in points
select GeographicCoordinatesToPoint(point.Latitude, point.Longitude))
.ToList();
// Gets the centroid (to have the normal of the vector space)
Vector3 centroid = GetCentroid(pointsIn3D );
// As we are on a sphere, the normal at a given point is the colinear to the vector going from the center of the sphere to the point.
Vector3 normal = (1f / centroid.Length) * centroid; // We want a unitary normal.
// Finally the area is computed using:
float area = GetArea(pointsIn3D, normal);
The Vector3 struct
public struct Vector3
{
public static readonly Vector3 Zero = new Vector3(0, 0, 0);
public readonly float X;
public readonly float Y;
public readonly float Z;
public float Length { return Math.Sqrt(X * X + Y * Y + Z * Z); }
public Vector3(float x, float y, float z)
{
X = x;
Y = y;
Z = z;
}
public static Vector3 operator +(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.X + vector2.X, vector1.Y + vector2.Y, vector1.Z + vector2.Z);
}
public static Vector3 operator -(Vector3 vector1, Vector3 vector2)
{
return new Vector3(vector1.X - vector2.X, vector1.Y - vector2.Y, vector1.Z - vector2.Z);
}
public static Vector3 operator *(float scalar, Vector3 vector)
{
return new Vector3(scalar * vector.X, scalar * vector.Y, scalar * vector.Z);
}
public static float DotProduct(Vector3 vector1, Vector3 vector2)
{
return vector1.X * vector2.X + vector1.Y * vector2.Y + vector1.Z * vector2.Z;
}
public static Vector3 CrossProduct(Vector3 vector1, Vector3 vector2)
{
return return new Vector3(vector1.Y * vector2.Z - vector1.Z * vector2.Y,
vector1.Z * vector2.X - vector1.X * vector2.Z,
vector1.X * vector2.Y - vector1.Y * vector2.X);
}
}
I fixed it with part of the code from Cédric and code from the internet.
I fixed it by using poly.Points and poly.LocalPoints.
The poly.Points are the latitude and longitude while the LocalPoints are points see to the center of the map on the screen.
the C# library has a function to calculate the distance (km) so I calculted the distance and then I calculated the distance in LocalPoints. Dived the localPoints throug the length in km and then you know how long 1 Localpoint is in km.
Code:
List<PointLatLng> firstTwoPoints = new List<PointLatLng>();
firstTwoPoints.Add(poly.Points[0]);
firstTwoPoints.Add(poly.Points[1]);
GMapPolygon oneLine = new GMapPolygon(firstTwoPoints,"testesddfsdsd"); //Create new polygone from messuring the distance.
double lengteLocalLine =
Math.Sqrt(((poly.LocalPoints[1].X - poly.LocalPoints[0].X)*(poly.LocalPoints[1].X - poly.LocalPoints[0].X)) +
((poly.LocalPoints[1].Y - poly.LocalPoints[0].Y)*(poly.LocalPoints[1].Y - poly.LocalPoints[0].Y))); //This calculates the length of the line in LocalPoints.
double pointInKm = oneLine.Distance / lengteLocalLine; //This gives me the length of 1 LocalPoint in km.
List<Carthesian> waarden = new List<Carthesian>();
//Here we fill the list "waarden" with the points.
//NOTE: the last value is NOT copied because this is handled in calculation method.
foreach (GPoint localPoint in poly.LocalPoints)
{
waarden.Add(new Carthesian(){X = (localPoint.X * pointInKm), Y = (localPoint.Y * pointInKm)});
}
MessageBox.Show("" + GetArea(waarden)*1000000);
}
//Method for calculating area
private double GetArea(IList<Carthesian> points)
{
if (points.Count < 3)
{
return 0;
}
double area = GetDeterminant(points[points.Count - 1].X , points[points.Count - 1].Y, points[0].X, points[0].Y);
for (int i = 1; i < points.Count; i++)
{
//Debug.WriteLine("Lng: " + points[i].Lng + " Lat:" + points[i].Lat);
area += GetDeterminant(points[i - 1].X, points[i - 1].Y , points[i].X, points[i].Y);
}
return Math.Abs(area / 2);
}
//Methode for getting the Determinant
private double GetDeterminant(double x1, double y1, double x2, double y2)
{
return x1 * y2 - x2 * y1;
}
//This class is just to make it nicer to show in code and it also was from previous tries
class Carthesian
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
}
Because we calculate the surface using 2D there is a small error, but for my application this is acceptable.
And thanks to Cédric for answering my question and helping me to fix the problem I had.
Its much easier to just use a backend database like SQL Server 2008 or MySql, sending the points of the polygon to the server in a query and return area, length, parimeter, intersection...etc.
If this is viable, search STArea() or STIntersect on Sql Server geography/geometry datatypes.
here is an example of something I have been working on.
using Microsoft.SqlServer.Types;
using System.Data.SqlClient;
GMap.NET.WindowsForms.GMapOverlay o = new GMapOverlay();
GMap.NET.WindowsForms.GMapOverlay o1 = new GMapOverlay();
List<PointLatLng> p = new List<PointLatLng>();
List<string> p1 = new List<string>();
//below adds a marker to the map upon each users click. At the same time adding that Lat/Long to a <PointLatLng> list
private void gMapControl1_MouseClick(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Right )
{
p.Add(new PointLatLng(Convert.ToDouble(gMapControl2.FromLocalToLatLng(e.X, e.Y).Lat), Convert.ToDouble(gMapControl2.FromLocalToLatLng(e.X, e.Y).Lng)));
p1.Add(gMapControl2.FromLocalToLatLng(e.X, e.Y).Lng + " " + gMapControl2.FromLocalToLatLng(e.X, e.Y).Lat);
GMarkerGoogle marker = new GMarkerGoogle(gMapControl2.FromLocalToLatLng(e.X, e.Y), GMarkerGoogleType.red_small);
// marker.Tag =(gMapControl1.FromLocalToLatLng(e.X, e.Y).Lng + " " + gMapControl1.FromLocalToLatLng(e.X, e.Y).Lat);
o.Markers.Add(marker);
gMapControl2.Overlays.Add(o);
}
}
//Then the below code will add that <PointLatLng> List to a SQL query and return Area and Centoid of polygon. Area is returned in Acres
private void gMapControl1_MouseDoubleClick(object sender, MouseEventArgs e)
{
if (e.Button == MouseButtons.Left)
{
try
{
o.Clear();
n = new GMapPolygon(p, "polygon");
n.Fill = new SolidBrush(Color.Transparent);
n.Stroke = new Pen(Color.Red, 1);
o.Polygons.Add(n);
gMapControl2.Overlays.Add(o);
StringBuilder a = new StringBuilder();
StringBuilder b = new StringBuilder();
p1.ToArray();
for (int i = 0; i != p1.Count; i++)
{
a.Append(p1[i].ToString() + ",");
}
a.Append(p1[0].ToString());
cs.Open();
SqlCommand cmd = new SqlCommand("Declare #g geography; set #g = 'Polygon((" + a + "))'; Select Round((#g.STArea() *0.00024711),3) As Area", cs);
SqlCommand cmd1 = new SqlCommand("Declare #c geometry; set #c =geometry::STGeomFromText('Polygon((" + a + "))',0); Select Replace(Replace(#c.STCentroid().ToString(),'POINT (',''),')','')AS Center", cs);
poly = "Polygon((" + a + "))";
SqlDataReader sdr = cmd.ExecuteReader();
while (sdr.Read())
{
txtArea.Text = sdr["Area"].ToString();
}
sdr.Close();
SqlDataReader sdr1 = cmd1.ExecuteReader();
while (sdr1.Read())
{
center = sdr1["Center"].ToString();
lat = center.Substring(center.IndexOf(" ") + 1);
lat = lat.Remove(9);
lon = center.Substring(0, (center.IndexOf(" ")));
lon = lon.Remove(10);
txtCenter.Text = lat + ", " + lon;
}
sdr1.Close();
}
catch (Exception ex)
{
MessageBox.Show("Please start the polygon over, you must create polygon in a counter-clockwise fasion","Counter-Clockwise Only!",MessageBoxButtons.OK,MessageBoxIcon.Error);
}
finally
{
p.Clear();
p1.Clear();
cs.Close();
o.Markers.Clear();
}
}
}

Categories