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).

Rotating cubes to face the origin using Quaternions

I'm in the process of setting up a relatively simple voxel-based world for a game. The high level idea is to first generate voxel locations following a fibonacci grid, then rotate the cubes such that the outer surface of the fibonacci grid resembles a sphere, and finally size the cubes such that they roughly cover the surface of the sphere (overlap is fine). See below the code for generating the voxels along the fibonacci grid:
public static Voxel[] CreateInitialVoxels(int numberOfPoints, int radius)
{
float goldenRatio = (1 + Mathf.Sqrt(5)) / 2;
Voxel[] voxels = new Voxel[numberOfPoints];
for (int i = 0; i < numberOfPoints; i++)
{
float n = i - numberOfPoints / 2; // Center at zero
float theta = 2 * Mathf.PI * n / goldenRatio;
float phi = (Mathf.PI / 2) + Mathf.Asin(2 * n / numberOfPoints);
voxels[i] = new Voxel(new Location(theta, phi, radius));
}
return voxels;
}
This generates a sphere that looks roughly like a staircase
So, my current approach to get this looking a bit more spherical is to basically rotate each cube in each pair of axes, then combine all of the rotations:
private void DrawVoxel(Voxel voxel, GameObject voxelContainer)
{
GameObject voxelObject = Instantiate<GameObject>(GetVoxelPrefab());
voxelObject.transform.position = voxel.location.cartesianCoordinates;
voxelObject.transform.parent = voxelContainer.transform;
Vector3 norm = voxel.location.cartesianCoordinates.normalized;
float xyRotationDegree = Mathf.Atan(norm.y / norm.x) * (180 / Mathf.PI);
float zxRotationDegree = Mathf.Atan(norm.z / norm.x) * (180 / Mathf.PI);
float yzRotationDegree = Mathf.Atan(norm.z / norm.y) * (180 / Mathf.PI);
Quaternion xyRotation = Quaternion.AngleAxis(xyRotationDegree, new Vector3(0, 0, 1));
Quaternion zxRotation = Quaternion.AngleAxis(zxRotationDegree, new Vector3(0, 1, 0));
Quaternion yzRotation = Quaternion.AngleAxis(yzRotationDegree, new Vector3(1, 0, 0));
voxelObject.transform.rotation = zxRotation * yzRotation * xyRotation;
}
The primary thing that I am getting caught on is that each of these rotations seems to work fine for me in isolation, but when combining them things tend to go a bit haywire (pictures below) I'm not sure exactly what the issue is. My best guess is that I've made some sign/rotation mismatch in my rotations so they don't combine right. I can get two working, but never all three together.
Above are the pictures of one and two successful rotations, followed by the error mode when I attempt to combine them. Any help either on telling me that the approach I'm following is too convoluted, or helping me understand what the right way to combine these rotations would be would be very helpful. Cartesian coordinate conversion below for reference.
[System.Serializable]
public struct Location
{
public float theta, phi, r;
public Vector3 polarCoordinates;
public float x, y, z;
public Vector3 cartesianCoordinates;
public Location(float theta, float phi, float r)
{
this.theta = theta;
this.phi = phi;
this.r= r;
this.polarCoordinates = new Vector3(theta, phi, r);
this.x = r * Mathf.Sin(phi) * Mathf.Cos(theta);
this.y = r * Mathf.Sin(phi) * Mathf.Sin(theta);
this.z = r * Mathf.Cos(phi);
this.cartesianCoordinates = new Vector3(x, y, z);
}
}

I managed to find a solution to this problem, though it's still not clear to me what the issue with the above code is.
Unity has an extremely handy function called Quaternion.FromToRotation that will generate the appropriate rotation if you simply pass in the appropriate destination vector.
In my case I was able to just do:
voxelObject.transform.rotation = Quaternion.FromToRotation(new Vector3(0, 0, 1), voxel.location.cartesianCoordinates);

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

Calculating Angle - Getting same value

in this program I calculate the angle for the right shoulder & right elbow, as well as the angle for the left shoulder & the left elbow. I print these values to the textbox in my WPF application. Now there is a problem: I get around 90° for every angle. I use Kinect for Windows and I am programming in C#. For more information leave a comment and I´ll answer.
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;
}
}
As you can see, I am calculating the angles with the "Dot-Product" and the acos. The (/PI*180)is to turn the number into the angle (0-360). I wonder what is wrong.

I found the answer: I have to normalize the Vectors in the AngleBetweenTwoVectors method. Then I get the real angle.

Calculate middle point of Bezier Curve

I have a function to draw Bezier Curve through three points. I have already 2 points (start and end) - A and B. How do I calculate middle point between those two points as middle point would be always a little higher or lower than linear function of those two points.
Example:
Any formulas, ideas would be great!

I think this is what you're looking for:
http://blog.sklambert.com/finding-the-control-points-of-a-bezier-curve/
It goes into detail on calculating the various points on a Bezier curve.
You may also be interested in this more specific example for your application:
http://www.codeproject.com/Articles/223159/Midpoint-Algorithm-Divide-and-Conquer-Method-for-D
If you really want to get into it, then I suggest this Primer:
http://pomax.github.io/bezierinfo/
Bezier curves are a bit more complicated than simple arcs. For an arc, you can just use this formula:
R = H/2 + W^2/8H
...which definitely won't work for a Bezier curve. On a Quadratic Bezier curve, for example, to calculate a point, you must use:
Sources: http://en.wikipedia.org/wiki/B%C3%A9zier_curve, Quadratic Bezier Curve: Calculate Point

Below is what I use to get the control point of a quad bezier curve. It should work for your problem where the control point is on the curve. It's in Swift but you should be able to convert it to another language easily. Basically at the midpoint of the line (whose points are point1 and point2) I work out a perpendicular line with the given length. Clockwise parameter determines which side of the line the point should fall on.
func getControlPointWithPoint1(point1:CGPoint, point2:CGPoint, length:CGFloat, clockwise:Bool) -> CGPoint {
let angle = getAngleWithPoint1(point1, point2:point2)
let direction = clockwise ? 1 : -1
let perpendicularAngle = angle + (CGFloat(direction) * CGFloat((M_PI / 2)))
let midPoint = getMidPointWithPoint1(point1, point2:point2)
return CGPointMake(midPoint.x + (cos(perpendicularAngle) * length), midPoint.y + (sin(perpendicularAngle) * length))
}
func getAngleWithPoint1(point1:CGPoint, point2:CGPoint) -> CGFloat {
return atan2((point2.y - point1.y), (point2.x - point1.x))
}
func getMidPointWithPoint1(point1:CGPoint, point2:CGPoint) -> CGPoint {
return CGPointMake((point1.x + point2.x) / 2, (point1.y + point2.y) / 2)
}
Below is how it would map to your diagram letters:
c = getControlPointWithPoint1(a, point2:b, length:h, clockwise:true)

following Mark's answer, here is the snippet in C#
public static Path DrawBezeireUsingTwoPoints(Point startPoint, Point endPoint)
{
Path path = new Path();
PathFigure pathFigure = new PathFigure();
// Set up the Path to insert the segments
PathGeometry pathGeometry = new PathGeometry();
BezierSegment bezeireSeg;
// Draw an ellipse passing by the 2 points and let the path cross it
Point beziereMidPoint = CalculateBezierePoint(startPoint, endPoint, true);
bezeireSeg = new BezierSegment(startPoint, beziereMidPoint, endPoint, true);
pathFigure.StartPoint = startPoint;
pathFigure.IsClosed = false;
pathFigure.Segments.Add(bezeireSeg);
pathGeometry.Figures.Add(pathFigure);
path.Data = pathGeometry;
path.Stroke = Brushes.Brown;
path.StrokeThickness = 2;
return path;
}

I would be happy if help you.
It is my solution.
Vector2 posA = sphereA.transform.position;
Vector2 posB = sphereB.transform.position;
Gizmos.color = Color.blue;
Gizmos.DrawLine(posA, posB);
float distance = Vector2.Distance(posA, posB);
Vector2 direction = (posB - posA).normalized;
Vector2 v2 = end - start;
var angle = Mathf.Atan2(v2.y, v2.x) * Mathf.Rad2Deg;
var midStartPos = posA + direction * (distance / 2f);
Gizmos.color = Color.red;
Gizmos.DrawSphere(midStartPos, 0.02f);
var height = 0.3f;
height = Mathf.Clamp(height, 0f, Vector2.Distance(posA, posB) * 0.5f);
angle = 90f + angle;
var goalDirection = new Vector2(Mathf.Cos(angle * Mathf.Deg2Rad), Mathf.Sin(angle * Mathf.Deg2Rad));
if (goalDirection.y < 0f)
{
goalDirection.x = -goalDirection.x;
goalDirection.y = Mathf.Abs(goalDirection.y);
}
var midEndPos = midStartPos + goalDirection * height;
Gizmos.color = Color.blue;
Gizmos.DrawLine(midStartPos, midEndPos);
Gizmos.color = Color.red;
Gizmos.DrawSphere(midEndPos, 0.02f);