Develop Reference

Extracting points and edge vectors

I am creating a program to generate a path for a CNC machine laser/plasma cutting. In it, the user should be able to cut shapes in the base element and be able to acquire the points and vectors of those cuts. I added the possibility to draw arrows (points and vectors) on selected walls according to which the tool should travel. This is based on obtaining the normal vector of the selected wall, which is used to determine the angle of cut.
Unfortunately, I do not know how to get the same effect on walls with a variable normal vector. An example of such an edge is the inclined cylinder. When I apply arrows to such an edge they all have the same vector.
Code sample:
public List<Mesh> DrawArrowsOnSelectedFace(Entity entity)
{
List<Mesh> arrowList = new List<Mesh>();
Brep ent = (Brep)entity;
for (int i = 0; i < ent.Faces.Length; i++)
{
if (ent.GetFaceSelection(i))
{
Surface[] sf = ent.Faces[i].ConvertToSurface(ent);
foreach (Surface surf in sf)
{
ICurve[] extractedEdges = surf.ExtractEdges();
Vector3D rotation = CalculatePerpenticularToNormalVector(surf);
foreach (ICurve curve in extractedEdges)
{
Point3D[] segmented = curve.GetPointsByLengthPerSegment(5);
for (int j = 1; j <= segmented.Length - 1; j++)
{
Point3D point1 = segmented[j - 1];
Mesh arrow = CreateArrow(point1, rotation);
arrowList.Add(arrow);
}
}
}
}
}
return arrowList;
}
private Vector3D CalculatePerpenticularToNormalVector(Surface surface)
{
Point3D point3D1 = new Point3D(surface.ControlPoints[0, 0].X, surface.ControlPoints[0, 0].Y, surface.ControlPoints[0, 0].Z);
Point3D point3D2 = new Point3D(surface.ControlPoints[0, 1].X, surface.ControlPoints[0, 1].Y, surface.ControlPoints[0, 1].Z);
Point3D point3D3 = new Point3D(surface.ControlPoints[1, 0].X, surface.ControlPoints[1, 0].Y, surface.ControlPoints[1, 0].Z);
Plane plane = new Plane(point3D1, point3D2, point3D3);
Vector3D equation = new Vector3D(plane.Equation.X, plane.Equation.Y, plane.Equation.Z);
Vector3D vectorZ = new Vector3D();
vectorZ.PerpendicularTo(Vector3D.AxisMinusY);
Vector3D result = CalculateRotation(vectorZ, equation);
result.Normalize();
return result;
}
private Mesh CreateArrow(Point3D point3D, Vector3D rotation)
{
if (point3D.Z >= -0.5)
{
return Mesh.CreateArrow(point3D, rotation, 0.3, 5, 0.35, 2, 36, Mesh.natureType.Smooth, Mesh.edgeStyleType.Sharp);
}
else return null;
}
private Vector3D CalculateRotation(Vector3D vector, Vector3D equation)
{
return vector - Vector3D.Dot(vector, equation) * equation;
}
What type should I best use for Boolean operations?
I also have a part of the code prepared where the arrows are drawn based on the common part of the basic element and the cut shape. Both of these shapes are BREPs. Unfortunately, this uses a lot of memory and takes some time.

You can convert the yellow face to a Surface using Brep.Faces[i].ConvertToSurface() and generating U or V isocurves of the resulting surface at equally spaced parameters using Surface.IsocurveU(t) or Surface.IsocurveU(t).

How to get the distance of two point geometry in meters using NetTopologySuite?

How to get the distance of two point geometry in meters using NetTopologySuite.
I have used the Distance() function, but I'm getting some values and I couldn't identify the unit of those values. Its for a c# application to neglect the tolerance value of 20 meters to be ignored.
using NetTopologySuite.Geometries;
using GeoAPI.Geometries;
private static double findistance()
{
var geomFactory = new GeometryFactory(new PrecisionModel(), 4326);
IGeometry geometry1 = geomFactory.CreatePoint(new Coordinate(12.977299, 77.571075));
IGeometry geometry2 = geomFactory.CreatePoint(new Coordinate(12.977277, 77.571258));
var distance = geometry1.Distance(geometry2);
return distance;
}
I need to get the distance calculated in meters.

Per docs, SRID 4326 stands for definition below
GEOGCS["WGS 84",
DATUM["WGS_1984",
SPHEROID["WGS 84",6378137,298.257223563,AUTHORITY["EPSG","7030"]],
AUTHORITY["EPSG","6326"]],
PRIMEM["Greenwich",0,AUTHORITY["EPSG","8901"]],
UNIT["degree",0.01745329251994328,AUTHORITY["EPSG","9122"]],
AUTHORITY["EPSG","4326"]]
UNIT["degree"... tells us that the result you are getting has values in degrees. We can calculate distance in meters, if we transform these points into Projected Coordinated system. ProjNET4GeoAPI package could do that for us.
private static double findDistance()
{
CoordinateTransformationFactory ctfac = new CoordinateTransformationFactory();
var from = GeographicCoordinateSystem.WGS84;
var to = ProjectedCoordinateSystem.WebMercator;
var trans = ctfac.CreateFromCoordinateSystems(from, to);
var mathTransform = trans.MathTransform;
var p1Coordinate = new GeoAPI.Geometries.Coordinate(12.977299, 77.571075);
var p2Coordinate = new GeoAPI.Geometries.Coordinate(12.977277, 77.571258);
p1Coordinate = mathTransform.Transform(p1Coordinate);
p2Coordinate = mathTransform.Transform(p2Coordinate);
return p1Coordinate.Distance(p2Coordinate);
}

you can use the Pythagoras theorem to calculate the distance.
Here are the steps:
Get Deltas
double int deltaX = Math.Abs(point1.X - point2.X);
double int deltaY = Math.Abs(point1.Y - point2.Y);
Pythagoras formula
double distance = Math.Sqrt((deltaX * deltaX) + (deltaY * deltaY));
If you want to calculate a 3D point it is the same steps except calculate the DeltaZ and then the formula is distance = X^2 + Y^2 + Z^2

Finding the true anomaly from state vectors

I'm attempting to convert from state vectors (position and speed) into Kepler elements, however I'm running into problems where a negative velocity or position will give me wrong results when trying to calculate true anomaly.
Here are the different ways I'm trying to calculate the True Anomaly:
/// <summary>
/// https://en.wikipedia.org/wiki/True_anomaly#From_state_vectors
/// </summary>
public static double TrueAnomaly(Vector4 eccentVector, Vector4 position, Vector4 velocity)
{
var dotEccPos = Vector4.Dot(eccentVector, position);
var talen = eccentVector.Length() * position.Length();
talen = dotEccPos / talen;
talen = GMath.Clamp(talen, -1, 1);
var trueAnomoly = Math.Acos(talen);
if (Vector4.Dot(position, velocity) < 0)
trueAnomoly = Math.PI * 2 - trueAnomoly;
return trueAnomoly;
}
//sgp = standard gravitational parameter
public static double TrueAnomaly(double sgp, Vector4 position, Vector4 velocity)
{
var H = Vector4.Cross(position, velocity).Length();
var R = position.Length();
var q = Vector4.Dot(position, velocity); // dot product of r*v
var TAx = H * H / (R * sgp) - 1;
var TAy = H * q / (R * sgp);
var TA = Math.Atan2(TAy, TAx);
return TA;
}
public static double TrueAnomalyFromEccentricAnomaly(double eccentricity, double eccentricAnomaly)
{
var x = Math.Sqrt(1 - Math.Pow(eccentricity, 2)) * Math.Sin(eccentricAnomaly);
var y = Math.Cos(eccentricAnomaly) - eccentricity;
return Math.Atan2(x, y);
}
public static double TrueAnomalyFromEccentricAnomaly2(double eccentricity, double eccentricAnomaly)
{
var x = Math.Cos(eccentricAnomaly) - eccentricity;
var y = 1 - eccentricity * Math.Cos(eccentricAnomaly);
return Math.Acos(x / y);
}
Edit: another way of doing it which Spectre pointed out:
public static double TrueAnomaly(Vector4 position, double loP)
{
return Math.Atan2(position.Y, position.X) - loP;
}
Positions are all relative to the parent body.
These functions all agree if position.x, position.y and velocity.y are all positive.
How do I fix these so that I get a consistent results when position and velocity are negitive?
Just to clarify: My angles appear to be sort of correct, just pointing in the wrong quadrant depending on the position and or velocity vectors.
Yeah so I was wrong, the above all do return the correct values after all.
So I found an edge case where most of the above calculations fail.
Given position and velocity:
pos = new Vector4() { X = -0.208994076275941, Y = 0.955838328099748 };
vel = new Vector4() { X = -2.1678187689294E-07, Y = -7.93096769486992E-08 };
I get some odd results, ie ~ -31.1 degrees, when I think it should return ` 31.1 (non negative). one of them returns ~ 328.8.
However testing with this position and velocity the results apear to be ok:
pos = new Vector4() { X = -0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(-25) };
See my answer for extra code on how I'm testing and the math I'm using for some of the other variables.
I'm going around in circles on this one. this is a result of a bug in my existing code that shows up under some conditions but not others.
I guess the real question now is WHY am I getting different results with position/velocity above that don't match to my expectations or each other?

Assuming 2D case... I am doing this differently:
compute radius of semi axises and rotation
so you need to remember whole orbit and find 2 most distant points on it that is major axis a. The minor axis b usually is 90 deg from major axis but to be sure just fins 2 perpendicularly most distant points on your orbit to major axis. So now you got both semi axises. The initial rotation is computed from the major axis by atan2.
compute true anomaly E
so if center is x0,y0 (intersection of a,b or center point of both) initial rotation is ang0 (angle of a) and your point on orbit is x,y then:
E = atan2(y-y0,x-x0) - ang0
However in order to match Newton/D'Alembert physics to Kepler orbital parameters you need to boost the integration precision like I did here:
Is it possible to make realistic n-body solar system simulation in matter of size and mass?
see the [Edit3] Improving Newton D'ALembert integration precision even more in there.
For more info and equations see:
Solving Kepler's equation
[Edit1] so you want to compute V I see it like this:
As you got your coordinates relative to parent you can assume they are already in focal point centered so no need for x0,y0 anymore. Of coarse if you want high precision and have more than 2 bodies (focal mass + object + proximity object(s) like moons) then the parent mass will no longer be in focal point of orbit but close to it ... and to remedy you need to use real focal point position so x0,y0 again... So how to do it:
compute center point (cx,cy) and a,b semi axises
so its the same as in previous text.
compute focal point (x0,y0) in orbit axis aligned coordinates
simple:
x0 = cx + sqrt( a^2 + b^2 );
y0 = cy;
initial angle ang0 of a
let xa,ya be the intersection of orbit and major axis a on the side with bigger speeds (near parent object focus). Then:
ang0 = atan2( ya-cy , xa-cx );
and finally the V fore any of yours x,y
V = atan2( y-y0 , x-x0 ) - ang0;

Ok so on further testing it appears my original calcs do all return the correct values, however when I was looking at the outputs I was not taking the LoP into account and basically not recognizing that 180 is essentially the same angle as -180.
(I was also looking at the output in radians and just didn't see what should have been obvious)
Long story short, I have a bug I thought was in this area of the code and got lost in the weeds.
Seems I was wrong above. see OP for edge case.
Here's some code I used to test these,
I used variations of the following inputs:
pos = new Vector4() { X = 0.25, Y = 0.25 };
vel = new Vector4() { X = Distance.KmToAU(-25), Y = Distance.KmToAU(25) };
And tested them with the following
double parentMass = 1.989e30;
double objMass = 2.2e+15;
double sgp = GameConstants.Science.GravitationalConstant * (parentMass + objMass) / 3.347928976e33;
Vector4 ev = OrbitMath.EccentricityVector(sgp, pos, vel);
double e = ev.Length();
double specificOrbitalEnergy = Math.Pow(vel.Length(), 2) * 0.5 - sgp / pos.Length();
double a = -sgp / (2 * specificOrbitalEnergy);
double ae = e * a;
double aop = Math.Atan2(ev.Y, ev.X);
double eccentricAnomaly = OrbitMath.GetEccentricAnomalyFromStateVectors(pos, a, ae, aop);
double aopD = Angle.ToDegrees(aop);
double directAngle = Math.Atan2(pos.Y, pos.X);
var θ1 = OrbitMath.TrueAnomaly(sgp, pos, vel);
var θ2 = OrbitMath.TrueAnomaly(ev, pos, vel);
var θ3 = OrbitMath.TrueAnomalyFromEccentricAnomaly(e, eccentricAnomaly);
var θ4 = OrbitMath.TrueAnomalyFromEccentricAnomaly2(e, eccentricAnomaly);
var θ5 = OrbitMath.TrueAnomaly(pos, aop);
double angleΔ = 0.0000001; //this is the "acceptable" amount of error, really only the TrueAnomalyFromEccentricAnomaly() calcs needed this.
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ1), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ2), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ3), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ4), angleΔ);
Assert.AreEqual(0, Angle.DifferenceBetweenRadians(directAngle, aop - θ5), angleΔ);
and the following to compare the angles:
public static double DifferenceBetweenRadians(double a1, double a2)
{
return Math.PI - Math.Abs(Math.Abs(a1 - a2) - Math.PI);
}
And eccentricity Vector found thus:
public static Vector4 EccentricityVector(double sgp, Vector4 position, Vector4 velocity)
{
Vector4 angularMomentum = Vector4.Cross(position, velocity);
Vector4 foo1 = Vector4.Cross(velocity, angularMomentum) / sgp;
var foo2 = position / position.Length();
return foo1 - foo2;
}
And EccentricAnomaly:
public static double GetEccentricAnomalyFromStateVectors(Vector4 position, double a, double linierEccentricity, double aop)
{
var x = (position.X * Math.Cos(-aop)) - (position.Y * Math.Sin(-aop));
x = linierEccentricity + x;
double foo = GMath.Clamp(x / a, -1, 1); //because sometimes we were getting a floating point error that resulted in numbers infinatly smaller than -1
return Math.Acos(foo);
}
Thanks to Futurogogist and Spektre for their help.

I am assuming you are working in two dimensions?
Two dimensional vectors of position p and velocity v. The constant K is the the product of the gravitational constant and the mass of the gravity generating body. Calculate the eccentricity vector
eccVector = (dot(v, v)*p - dot(v, p)*v) / K - p / sqrt(dot(p, p));
eccentricity = sqrt(dot(eccVector, eccVector));
eccVector = eccVector / eccentricity;
b = { - eccVector.y, eccVector.x}; //unit vector perpendicular to eccVector
r = sqrt(dot(p, p));
cos_TA = dot(p, eccVector) / r; \\ cosine of true anomaly
sin_TA = dot(p, b) / r; \\ sine of true anomaly
if (sin_TA >= 0) {
trueAnomaly = arccos(cos_TA);
}
else if (sin_TA < 0){
trueAnomaly = 2*pi - arccos(cos_TA);
}

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;
}

DrawBone Angle variable using Kinect

when we want to draw Bones into XNA (using KnectSDK and C#), you have to calculate differences between two joints and draw bones between them. this is the function for drawing Bones:
private void DrawBone(JointCollection joints, JointType startJoint, JointType endJoint)
{
Vector2 start = this.mapMethod(joints[startJoint].Position);
Vector2 end = this.mapMethod(joints[endJoint].Position);
Vector2 diff = end - start;
Vector2 scale = new Vector2(1.0f, diff.Length() / this.boneTexture.Height);
float angle = (float)Math.Atan2(diff.Y, diff.X) - MathHelper.PiOver2;
Color color = Color.LightGreen;
if (joints[startJoint].TrackingState != JointTrackingState.Tracked ||
joints[endJoint].TrackingState != JointTrackingState.Tracked)
{
color = Color.Gray;
}
this.SharedSpriteBatch.Draw(this.boneTexture, start, null, color, angle, this.boneOrigin, scale, SpriteEffects.None, 1.0f);
}
I just want to understand the way of calculating angle and how the angle formula works
Thanks

Here is the code to calculate the angle between two vectors (Explanation below code):
public class Angles
{
public double AngleBetweenTwoVectors(Vector3D vectorA, Vector3D vectorB)
{
double dotProduct = 0.0;
dotProduct = Vector3D.DotProduct(vectorA, vectorB);
return (double)Math.Acos(dotProduct)/Math.PI*180;
}
public double[] GetVector(Skeleton skeleton)
{
Vector3D ShoulderCenter = new Vector3D(skeleton.Joints[JointType.ShoulderCenter].Position.X, skeleton.Joints[JointType.ShoulderCenter].Position.Y, skeleton.Joints[JointType.ShoulderCenter].Position.Z);
Vector3D RightShoulder = new Vector3D(skeleton.Joints[JointType.ShoulderRight].Position.X, skeleton.Joints[JointType.ShoulderRight].Position.Y, skeleton.Joints[JointType.ShoulderRight].Position.Z);
Vector3D LeftShoulder = new Vector3D(skeleton.Joints[JointType.ShoulderLeft].Position.X, skeleton.Joints[JointType.ShoulderLeft].Position.Y, skeleton.Joints[JointType.ShoulderLeft].Position.Z);
Vector3D RightElbow = new Vector3D(skeleton.Joints[JointType.ElbowRight].Position.X, skeleton.Joints[JointType.ElbowRight].Position.Y, skeleton.Joints[JointType.ElbowRight].Position.Z);
Vector3D LeftElbow = new Vector3D(skeleton.Joints[JointType.ElbowLeft].Position.X, skeleton.Joints[JointType.ElbowLeft].Position.Y, skeleton.Joints[JointType.ElbowLeft].Position.Z);
Vector3D RightWrist = new Vector3D(skeleton.Joints[JointType.WristRight].Position.X, skeleton.Joints[JointType.WristRight].Position.Y, skeleton.Joints[JointType.WristRight].Position.Z);
Vector3D LeftWrist = new Vector3D(skeleton.Joints[JointType.WristLeft].Position.X, skeleton.Joints[JointType.WristLeft].Position.Y, skeleton.Joints[JointType.WristLeft].Position.Z);
/* ShoulderCenter.Normalize();
RightShoulder.Normalize();
LeftShoulder.Normalize();
RightElbow.Normalize();
LeftElbow.Normalize();
RightWrist.Normalize();
LeftWrist.Normalize();
if (skeleton.Joints[JointType.ShoulderCenter].TrackingState == JointTrackingState.Tracked) {
}
*/
double AngleRightElbow = AngleBetweenTwoVectors(RightElbow - RightShoulder, RightElbow - RightWrist);
double AngleRightShoulder = AngleBetweenTwoVectors(RightShoulder - ShoulderCenter, RightShoulder - RightElbow);
double AngleLeftElbow = AngleBetweenTwoVectors(LeftElbow - LeftShoulder, LeftElbow - LeftWrist);
double AngleLeftShoulder = AngleBetweenTwoVectors(LeftShoulder - ShoulderCenter, LeftShoulder - LeftElbow);
double[] Angles = {AngleRightElbow, AngleRightShoulder, AngleLeftElbow, AngleLeftShoulder};
return Angles;
}
}
First of all we'll have a look at the method GetVector(Skeleton skeleton). In this method we first define our Vector3D(for the joints you choose). We then call the method AngleBetweenTwoVectors and give it two parameters.
But attention. If we would just give it the Vector of the Joints, we would not get the correct angle. We first have to substract the surrounding Vectors from the Vector (Vector we want to get the angle from). We then pass the two Vectors to the method AngleBetweenTwoVectors.
In this method we first have to calculate the dot-product. For more information on the dot-product click here.
With the dot-product we can calculate the angle. To do that we just need to use the arcos() method.
Another thing I struggled with: The assemblies.
You need to import:
using System.Windows.Media;
using Microsoft.Kinect;
using Microsoft.Kinect.Toolkit.Fusion;
using System.Windows.Media.Media3D;
To get the [...].Toolkit.Fusion assembly go to "Add Reference" and hit "Browse". You will then guide to the assembly directory from the MicrosoftSDK's/Kinect/Developer Toolkit v1.8.0/Assemblies. Import the assembly and add it to your project.