Develop Reference

C# intersect a line bettween 2 Vector3 point on a plane

I have a line going bettween two Vector3 points and I want to find when the line is intersected at a height along the Z axis.
I am trying to write a function to calculate the intersection point.
void Main()
{
Vector3 A = new Vector3(2.0f, 2.0f, 2.0f);
Vector3 B = new Vector3(7.0f, 10.0f, 6.0f);
float Z = 3.0f;
Vector3 C = getIntersectingPoint(A, B, Z);
//C should be X=3.25, Y=4.0, Z=3.0
}
But trying to figure out how to do the math to handle possible negative numbers correctly is really starting to confuse me.
This is what I have and the moment, but this isn't correct.
public static Vector3 getIntersectingPoint(Vector3 A, Vector3 B, float Z)
{
// Assume z is bettween A and B and we don't need to validate
// Get ratio of triangle hight, height Z divided by (Za to Zb)
("absolute value: " + Math.Abs(A.Z-B.Z)).Dump();
("z offset: " + (Math.Abs(Z-B.Z)<Math.Abs(A.Z-Z)?Math.Abs(Z-B.Z):Math.Abs(A.Z-Z))).Dump();
float ratio = (Math.Abs(Z-B.Z)<Math.Abs(A.Z-Z)?Math.Abs(Z-B.Z):Math.Abs(A.Z-Z))/Math.Abs(A.Z-B.Z);
("ratio: " + ratio.ToString()).Dump();
float difX = ratio*Math.Abs(A.X-B.X);//this still needs to be added to or taken from the zero point offset
("difX: " + difX.ToString()).Dump();
float difY = ratio*Math.Abs(A.Y-B.Y);//this still needs to be added to or taken from the zero point offset
("difY: " + difY.ToString()).Dump();
float X = difX + (A.X<B.X?A.X:B.X);
("X: " + X).Dump();
float Y = difY + (A.Y<B.Y?A.Y:B.Y);
("Y: " + Y).Dump();
return new Vector3(X,Y,Z);
}
Does anyone know if there are any Math libraries that will already do this or examples that show how to do this that I can follow?

You have the starting (2.0f) and ending (6.0f) Z coordinates. The Z distance between the two points is 4.0f. You want to know the X and Y coordinates at the point where Z is 3.0f.
Remember that Z changes linearly along the segment. The segment is 4 units long, The point you're interested in is 1 unit from the start, or 1/4 of the length of the segment.
The X distance of the entire segment is 7.0 - 2.0, or 5 units. 1/4 of 5 is 1.25, so the X coordinate at the intersection is 3.25.
The Y distance of the entire segment is 8. 1/4 of 8 is 2. So the Y coordinate of the intersection point is 6.0.
The intersection point is (3.25f, 6.0f, 3.0f).
How to compute:
// start is the starting point
// end is the ending point
// target is the point you're looking for.
// It's pre-filled with the Z coordinate.
zDist = Math.Abs(end.z - start.z);
zDiff = Math.Abs(target.z - start.z);
ratio = zDiff / zDist;
xDist = Math.Abs(end.x - start.x);
xDiff = xDist * ratio;
xSign = (start.x < end.x) ? 1 : -1;
target.x = start.x + (xDiff * xSign);
yDist = Math.Abs(end.y - start.y);
yDiff = yDist * ratio;
ySign = (start.y < end.y) ? 1 : -1;
target.y = start.y + (yDiff * ySign);
Come to think of it, the whole sign thing shouldn't be necessary. Consider this, when end.x = 10 and start.x = 18:
xDist = end.x - start.x; // xDist = -8
xDiff = xDist * ratio; // xDiff = -2
target.x = start.x + xDiff; // target.x = 18 + (-2) = 16
Yeah, no need for sign silliness.
Also no need for the calls to Math.Abs when computing the ratio. We know that zDist and zDiff will both have the same sign, so ratio will always be positive.

find the least angle of a line to X axis in C#

Let's say we define two vectors by subtracting the end points of two lines.
V1 = Pa - Pb;
V2 = Pc - Pd;
and we define the X axis as the following vector.
var V = new System.Windows.Vector(1, 0);
How can we know which one of the two two vectors V1 and V2 has the least angle to X axis.

You have two options: You can calculate the angles between V1 and V and V2 and V with the AngleBetween Function:
var angle1 = Vector.AngleBetween(V1,V);
var angle2 = Vector.AngleBetween(V2,V);
if (angle1 < angle2) {
//V1 is closer to V
}else{
//V2 is closer to V
}
or you could also normalize your vectors and compare their y values afterwards:
V1.Normalize();
V2.Normalize();
if(Math.Abs(V1.Y) < Math.Abs(V2.Y)){
//V1 is closer
}else{
//V2 is closer
}
I would prefer the first method, since the Normalize() function actually changes the original Vector so you would need to make a copy if you wanted to use them afterwards. Also you can use the first version in order to compare them to any other vector V without needing to adjust the code.
Edit: Actually the first version only chooses the vector with the smallest angle to the V vector and not to the x-axis. So if you want to compare to the axis instead of just the vector you should do the angle calculation with copies of V1 and V2 with absolute x and y values:
var ref1 = new System.Windows.Vector(Math.Abs(V1.X),Math.Abs(V1.Y));
var ref2 = new System.Windows.Vector(Math.Abs(V2.X),Math.Abs(V2.Y));
var angle1 = Vector.AngleBetween(ref1,V);
var angle2 = Vector.AngleBetween(ref2,V);
if (angle1 < angle2) {
//V1 is closer to V
}else{
//V2 is closer to V
}
Looking at it like this, the normalization method might be the better choice after all. (But be sure to call Normalize() on copies)

Normalize both and find one with higher absolute value of x.
Alternatively, angle between two vectors a and b is arccos((a.x*b.x+a.y*b.y)/(a.Length*b.Length)) (undefined if one of vectors is zero)

Check the following solution. May fulfill your query.Add reference Windowsbase to project. Very Important.
using System;
using System.Windows;
namespace Vectors
{
class Program
{
static void Main(string[] args)
{
// Define Points
Point Pa = new Point(5.0,1.0);
Point Pb = new Point(10.0,3.0);
Point Pc = new Point(7.0,10.0);
Point Pd = new Point(1.0,3.0);
Vector V1 = Pa - Pb;
Vector V2 = Pc - Pd;
Vector V = new Vector(1, 0);
double Phi1 = Math.Atan2(V1.Y, V1.X)*180/Math.PI;
double Phi2 = Math.Atan2(V2.Y, V2.X)*180/Math.PI;
// Check for -ve angle and take 180 degree complement.
Phi1 = (Phi1 >= 0) ? Phi1 : 180 + Phi1;
Phi2 = (Phi2 >= 0) ? Phi2 : 180 + Phi2;
if(Phi1<=Phi2)
{
Console.WriteLine("Vector V1 has a least angle");
}
else
{
Console.WriteLine("Vector V2 has a least angle");
}
Console.ReadLine();
}
}
}

double compare() {
Point p0 = new Point(0, 0);
Point p1 = new Point(100, 100);
Point p2 = new Point(100, -100);
Vector v0 = new System.Windows.Vector(1, 0);
var v1 = p1 - p0;
var v2 = p2 - p0;
var value1 = Vector.AngleBetween(v0, v1); //value = 45
var value2 = Vector.AngleBetween(v0, v2); //value = -45
value1 = Math.Abs(value1);
value2 = Math.Abs(value2);
/* Or if you want to compare the value of the angle
if (value1 < 0)
value1 += 360;
if (value2 < 0)
value2 += 360;
*/
return Math.Min(value1,value2);
}

Calculating Lean Angle with core motion of iOS device

I am trying to calculate the lean angle/inclination of my iOS device.
I did a lot of research and found a way how to give me the most accurate lean angle/inclincation. I used quaternion to calculate it.
This is the code that I use to calculate it.
public void CalculateLeanAngle ()
{
motionManager = new CMMotionManager ();
motionManager.DeviceMotionUpdateInterval = 0.02; // 50 Hz
if (motionManager.DeviceMotionAvailable) {
motionManager.StartDeviceMotionUpdates (CMAttitudeReferenceFrame.XArbitraryZVertical, NSOperationQueue.CurrentQueue, (data, error) => {
CMQuaternion quat = motionManager.DeviceMotion.Attitude.Quaternion;
double x = quat.x;
double y = quat.y;
double w = quat.w;
double z = quat.z;
double degrees = 0.0;
//Roll
double roll = Math.Atan2 (2 * y * w - 2 * x * z, 1 - 2 * y * y - 2 * z * z);
Console.WriteLine("Roll: " + Math.Round(-roll * 180.0/Constants.M_PI));
degrees = Math.Round (-applyKalmanFiltering (roll) * 180.0 / Constants.M_PI);
string degreeStr = string.Concat (degrees.ToString (), "°");
this.LeanAngleLbl.Text = degreeStr;
});
}
public double applyKalmanFiltering (double yaw)
{
// kalman filtering
if (motionLastYaw == 0) {
motionLastYaw = yaw;
}
float q = 0.1f; // process noise
float r = 0.1f; // sensor noise
float p = 0.1f; // estimated error
float k = 0.5f; // kalman filter gain
double x = motionLastYaw;
p = p + q;
k = p / (p + r);
x = x + k * (yaw - x);
p = (1 - k) * p;
motionLastYaw = x;
return motionLastYaw;
}
This works perfect when you walk and tilt your device. But when I drive my car something happens that the quaternion isn't giving me the correct lean angle/inclincation. It just gives me 180°.. Or suddenly it shows me a totally wrong lean angle/inclination.. Like when I am standing still (at trafic lights) it shows me 23°... Then after driving a bit it works again or it shows again 180°.
Could it be that the quaternion is effected by the acceleration of my car? So that because my car is driving at a certain speed it isn't giving me the correct value?
Does anyone have any solution for this?
I would like to calculate my lean angle/inclincation when I drive my bike/car. So I really want to know how to calulate the right lean angle/inclincation independent if I drive my car/bike or not.
Thanks in advance!

Waypoint generator - Calculating round trip based on rules

I'm trying to work on a piece of code to return a list of waypoints which represent a flight path for a AI raid to fly. Unfortunately maths and trig were never my strong point so I've been googling and not getting very far very fast.
I have the following code
public static Point2d CalculateCoordinate(double Angle, double Distance)
{
Point2d coord = new Point2d(Distance * Math.Cos(Angle), Distance * Math.Sin(Angle));
return coord;
}
public static double GetAngle(Point2d coord1, Point2d coord2, Point2d coord3, Point2d coord4)
{
double result = 0.0;
result = (Math.Atan2(coord2.y - coord1.y, coord2.x - coord1.x) - Math.Atan2(coord4.y - coord3.y, coord4.x - coord3.x)) * (180 / Math.PI);
if(result<0)
{
result = result + 360;
}
return result;
}
public static List<Point2d> GenerateWaypoints(Point2d Startpoint, Point2d Target)
{
List<Point2d> waypoints = new List<Point2d>();
double tempDistance = 0.0;
double tempAngle = 30.0 * ( Math.PI/180);
bool ok = false;
double distancemodifier = 0.8;
waypoints.Add(Startpoint);
tempDistance = Startpoint.distance(ref Target)*distancemodifier;
Console.WriteLine(tempDistance.ToString());
Console.WriteLine(ReturnStringFromP2D(Startpoint));
Console.WriteLine(ReturnStringFromP2D(Target));
Point2d tempPoint2d = CalculateCoordinate(tempAngle, tempDistance);
Console.WriteLine(ReturnStringFromP2D(tempPoint2d));
while (!ok)
{
tempPoint2d = CalculateCoordinate(tempAngle, tempDistance);
if (GetAngle(Startpoint, tempPoint2d, tempPoint2d, Target) > 30.00 || tempPoint2d.distance(ref Target) < 10000)
{
distancemodifier = distancemodifier - 0.05;
tempDistance = Startpoint.distance(ref Target) * distancemodifier;
}
}
ok = false;
waypoints.Add(tempPoint2d);
waypoints.Add(Target);
tempAngle = tempAngle + 30.0;
tempDistance = Target.distance(ref Startpoint) * distancemodifier;
tempPoint2d = CalculateCoordinate(tempAngle, tempDistance);
while (!ok)
{
tempPoint2d = CalculateCoordinate(tempAngle, tempDistance);
if (GetAngle(Target, tempPoint2d, tempPoint2d, Startpoint) > 30.00)
{
distancemodifier = distancemodifier - 0.05;
tempDistance = Target.distance(ref Startpoint) * distancemodifier;
}
}
waypoints.Add(tempPoint2d);
return waypoints;
}
The issue is that I'm not getting anything out of it that makes sense. The rules I want to apply are that no turn in the flightpath should be larger than 30 degrees. I have the start position, the target position. I want to make sure that the first turn is further than 10km from the target position.
EDIT: OK, here's what I am actually trying to achieve.
Using a 1km grid.
Taking Start point to be 0, 10 and target point to be 10,40, I want to generate a set of waypoints that go from start, to an intermediate step between start and target which should generate a no more than 30 degree angle (but not be a straight line) between the two legs and no closer to target than 5km. Then to target, then back to the start point, with the rule that no angle should be greater than 30 degrees. Therefore we might get the following (worked out roughly on paper so there might be an error but it should be indicative of what I'm trying to achieve.)
Start 0,10
Intermediate 22,20
Target 10,40
Intermediate -10,48
Intermediate -25,40
Intermediate -35,28
Intermediate -35,14
Intermediate -20,5
Start 0,10
It seems to me that I need three methods, one to return the bearing between two points (used at the start to get the initial direction from start to target - and then modify away from a straight line), the second to calculate the angle between two lines (used on every step to check the previous angle doesn't exceed 30 degrees), and the third to generate a coordinate given a bearing and a distance from the current location.
From what I understand, my GetAngle() above is correct, and cut down to this will work for bearings between two points??
public static double GetBearing(Point2d coord1, Point2d coord2) {
double result = 0.0;
result = Math.Atan2(coord2.y - coord1.y, coord2.x - coord1.x) * (180 / Math.PI);
if(result<0)
{
result = result + 360;
}
return result;
}

Calculate coordinates of a regular polygon's vertices

I am writing a program in which I need to draw polygons of an arbitrary number of sides, each one being translated by a given formula which changes dynamically. There is some rather interesting mathematics involved but I am stuck on this probelm.
How can I calculate the coordinates of the vertices of a regular polygon (one in which all angles are equal), given only the number of sides, and ideally (but not neccessarily) having the origin at the centre?
For example: a hexagon might have the following points (all are floats):
( 1.5 , 0.5 *Math.Sqrt(3) )
( 0 , 1 *Math.Sqrt(3) )
(-1.5 , 0.5 *Math.Sqrt(3) )
(-1.5 , -0.5 *Math.Sqrt(3) )
( 0 , -1 *Math.Sqrt(3) )
( 1.5 , -0.5 *Math.Sqrt(3) )
My method looks like this:
void InitPolygonVertexCoords(RegularPolygon poly)
and the coordinates need to be added to this (or something similar, like a list):
Point[] _polygonVertexPoints;
I'm interested mainly in the algorithm here but examples in C# would be useful. I don't even know where to start. How should I implement it? Is it even possible?!
Thank you.

for (i = 0; i < n; i++) {
printf("%f %f\n",r * Math.cos(2 * Math.PI * i / n), r * Math.sin(2 * Math.PI * i / n));
}
where r is the radius of the circumsribing circle. Sorry for the wrong language No Habla C#.
Basically the angle between any two vertices is 2 pi / n and all the vertices are at distance r from the origin.
EDIT:
If you want to have the center somewher other than the origin, say at (x,y)
for (i = 0; i < n; i++) {
printf("%f %f\n",x + r * Math.cos(2 * Math.PI * i / n), y + r * Math.sin(2 * Math.PI * i / n));
}

The number of points equals the number of sides.
The angle you need is angle = 2 * pi / numPoints.
Then starting vertically above the origin with the size of the polygon being given by radius:
for (int i = 0; i < numPoints; i++)
{
x = centreX + radius * sin(i * angle);
y = centreY + radius * cos(i * angle);
}
If your centre is the origin then simply ignore the centreX and centreY terms as they'll be 0,0.
Swapping the cos and sin over will point the first point horizontally to the right of the origin.

Sorry, I dont have a full solution at hand right now, but you should try looking for 2D-Rendering of Circles. All classic implementations of circle(x,y,r) use a polygon like you described for drawing (but with 50+ sides).

Say the distance of the vertices to the origin is 1. And say (1, 0) is always a coordinate of the polygon.
Given the number of vertices (say n), the rotation angle required to position the (1, 0) to the next coordinate would be (360/n).
The computation required here is to rotate the coordinates. Here is what it is; Rotation Matrix.
Say theta = 360/n;
[cos(theta) -sin(theta)]
[sin(theta) cos(theta)]
would be your rotation matrix.
If you know linear algebra you already know what i mean. If dont just have a look at Matrix Multiplication

One possible implementation to generate a set of coordinates for regular polygon is to:
Define polygon center, radius and first vertex1. Rotate the vertex n-times2 at an angle of: 360/n.
In this implementation I use a vector to store the generated coordinates and a recursive function to generate them:
void generateRegularPolygon(vector<Point>& v, Point& center, int sidesNumber, int radius){
// converted to radians
double angRads = 2 * PI / double(sidesNumber);
// first vertex
Point initial(center.x, center.y - radius);
rotateCoordinate(v, center, initial, angRads, sidesNumber);
}
where:
void rotateCoordinate(vector<Point>& v, Point& axisOfRotation, Point& initial, double angRads, int numberOfRotations){
// base case: number of transformations < 0
if(numberOfRotations <= 0) return;
else{
// apply rotation to: initial, around pivot point: axisOfRotation
double x = cos(angRads) * (initial.x - axisOfRotation.x) - sin(angRads) * (initial.y - axisOfRotation.y) + axisOfRotation.x;
double y = sin(angRads) * (initial.x - axisOfRotation.x) + cos(angRads) * (initial.y - axisOfRotation.y) + axisOfRotation.y;
// store the result
v.push_back(Point(x, y));
rotateCoordinate(v, axisOfRotation, Point(x,y), angRads, --numberOfRotations);
}
}
Note:
Point is a simple class to wrap the coordinate into single data structure:
class Point{
public:
Point(): x(0), y(0){ }
Point(int xx, int yy): x(xx), y(yy) { }
private:
int x;
int y;
};
1 in terms of (relative to) the center, radius. In my case the first vertex is translated from the centre up horizontally by the radius lenght.
2 n-regular polygon has n vertices.

The simple method is:
Let's take N-gone(number of sides) and length of side L. The angle will be T = 360/N.
Let's say one vertices is located on origin.
* First vertex = (0,0)
* Second vertex = (LcosT,LsinT)
* Third vertex = (LcosT+Lcos2T, LsinT+Lsin2T)
* Fourth vertex = (LcosT+Lcos2T+Lcos3T, LsinT+Lsin2T+Lsin3T)
You can do in for loop

hmm if you test all the versions that are listed here you'll see that the implementation is not good. you can check the distance from the center to each generated point of the polygon with : http://www.movable-type.co.uk/scripts/latlong.html
Now i have searched a lot and i could not find any good implementation for calculating a polyogon using the center and the radius...so i went back to the math book and tried to implement it myself. In the end i came up with this...wich is 100% good:
List<double[]> coordinates = new List<double[]>();
#region create Polygon Coordinates
if (!string.IsNullOrWhiteSpace(bus.Latitude) && !string.IsNullOrWhiteSpace(bus.Longitude) && !string.IsNullOrWhiteSpace(bus.ListingRadius))
{
double lat = DegreeToRadian(Double.Parse(bus.Latitude));
double lon = DegreeToRadian(Double.Parse(bus.Longitude));
double dist = Double.Parse(bus.ListingRadius);
double angle = 36;
for (double i = 0; i <= 360; i += angle)
{
var bearing = DegreeToRadian(i);
var lat2 = Math.Asin(Math.Sin(lat) * Math.Cos(dist / earthRadius) + Math.Cos(lat) * Math.Sin(dist / earthRadius) * Math.Cos(bearing));
var lon2 = lon + Math.Atan2(Math.Sin(bearing) * Math.Sin(dist / earthRadius) * Math.Cos(lat),Math.Cos(dist / earthRadius) - Math.Sin(lat) * Math.Sin(lat2));
coordinates.Add(new double[] { RadianToDegree(lat2), RadianToDegree(lon2) });
}
poly.Coordinates = new[] { coordinates.ToArray() };
}
#endregion
If you test this you'll see that all the points are at the exact distance that you give ( radius ). Also don't forget to declare the earthRadius.
private const double earthRadius = 6371.01;
This calculates the coordinates of a decagon. You see the angle used is 36 degrees. You can split 360 degrees to any number of sides that you want and put the result in the angle variable.
Anyway .. i hope this helps you #rmx!