I'm quite new to coding in general. I have found some answers for this question but the answers seem advanced for me.
I'm trying to write my own Finite Element Project. For this I would like to write a method that checks if random 4 nodes given as input form a convex quadrilateral.
My method is supposed to look like this:
private bool IsConvex(Node[4] corners)
{
bool isConvex;
//CODE//
return isConvex;
}
the Node class is defined by three public properties referring to their coordinates (.coordX, .coordY, .coordZ)
In order to know if a quadrilateral is convex or not, you can make a triangle of three points and see if the fourth point is located inside that triangle or not. If you manage finding one triangle, which contains the fourth point, then you don't have a convex quadrilateral.
Ok, and how can you know if a point is located inside a triangle?
Well, you start by determining at which side a point is located compared to a vector.
Come again?
Well, for each vector, you can find out if a point is located at the left side or at the right side: you just rotate the vector back to the Y-axis, you do the same with the point and if the X coordinate of the point is negative your point is located at the left side, otherwise it's at the right side, like in these three cases (left, left and right):
Once you have figured that out, you define a point being inside a triangle if, after having described the triangle as a triangle of vectors, your point is at the same side of all vectors, like in this example (be aware that your triangle consists of the vectors AB, BC and CA: the points must follow up each other):
Good luck
First a little helper class to handle things related to triangles made up of three nodes.
using System.Numerics;
public readonly struct Triangle
{
public const float DistanceTolerance = 1e-6f;
public Triangle(Vector3 a, Vector3 b, Vector3 c)
{
A = a;
B = b;
C = c;
}
public Vector3 A { get; }
public Vector3 B { get; }
public Vector3 C { get; }
private Vector3 AreaVector { get => (Vector3.Cross(A, B) + Vector3.Cross(B, C) + Vector3.Cross(C, A)) / 2; }
public float Area { get => AreaVector.Length(); }
public Vector3 Normal { get => Vector3.Normalize(AreaVector); }
public float DistanceTo(Vector3 point) => Vector3.Dot(Normal, point - A);
public Vector3 Project(Vector3 point)
{
// A projected point lies on the plane defined by the three veertices A,B,C
Vector3 n = Normal;
float d = Vector3.Dot(n, point - A);
return point - n * d;
}
public void Barycentric(Vector3 P, out (float w_A, float w_B, float w_C) coordinates)
{
Vector3 n = Vector3.Cross(A, B) + Vector3.Cross(B, C) + Vector3.Cross(C, A);
float w_A = Vector3.Dot(n, Vector3.Cross(P, B) + Vector3.Cross(B, C) + Vector3.Cross(C, P));
float w_B = Vector3.Dot(n, Vector3.Cross(A, P) + Vector3.Cross(P, C) + Vector3.Cross(C, A));
float w_C = Vector3.Dot(n, Vector3.Cross(A, B) + Vector3.Cross(B, P) + Vector3.Cross(P, A));
float sum = w_A + w_B + w_C;
coordinates = (w_A / sum, w_B / sum, w_C / sum);
}
public bool Contains(Vector3 P)
{
if (Math.Abs(DistanceTo(P)) <= DistanceTolerance)
{
Barycentric(P, out var coordinates);
return coordinates.w_A >= 0 && coordinates.w_A <= 1
&& coordinates.w_B >= 0 && coordinates.w_B <= 1
&& coordinates.w_C >= 0 && coordinates.w_C <= 1;
}
return false;
}
}
If you are not familiar with barycentric coordinates, they are the linear combinations of the vertices that make up an interior (or exterior) point.
For example if a point is defined as P = 0.3*A + 0.5*B + 0.2*C then the barycentric coordinates of P are (0.3,0.5,0.2). The only restriction here is that the sum of the barycentric coordinates must equal to 1.
A point P is interior to the triangle ABC if all the barycentric coordinates of P are between 0 and 1.
This is the rule that I am using to write the Triangle.Contains(point) function. I also check to see if the point is on the same plane as the triangle.
Now to get to the algorithm to check if an n-gon is convex, all I have to do is take 3 vertices at a time, and check that all remaining other vertices are exterior to those three.
public static bool IsConvex(Vector3[] nodes)
{
for (int i = 0; i < nodes.Length; i++)
{
// pick three nodes at a time i,j,k
var j = (i + 1) % nodes.Length;
var k = (i + 2) % nodes.Length;
var A = nodes[i];
var B = nodes[j];
var C = nodes[k];
// deefine triangle ABC from three nodes
var trig = new Triangle(A, B, C);
// check nodes after the three and wrap around to grab first nodes also
for (int r = 3; r < nodes.Length; r++)
{
var P = nodes[(r + i) % nodes.Length];
// if _any_ node is interior to ABC then non-convex
if (trig.Contains(P))
{
return false;
}
}
}
return true;
}
and some test code to make sure it all works as intended.
static readonly Random rng = new Random();
static void Main(string[] args)
{
// Generate a random 3D triangle
var trig = new Triangle(
new Vector3(10 * (float)rng.NextDouble(), 0, 0),
new Vector3(0, 10 * (float)rng.NextDouble(), 0),
new Vector3(0, 0, 10 * (float)rng.NextDouble()));
// Generate an interior point (in the plane)
var point1 = 0.3f * trig.A + 0.5f * trig.B + 0.2f * trig.C;
// Check that it is contained inside the triangle
Debug.Assert(trig.Contains(point1));
// Generate an exterior point (on the plane)
var point2 = -0.3f * trig.A + 0.5f * trig.B + 0.8f * trig.C;
// Check that it is not contained inside the triangle
Debug.Assert(!trig.Contains(point2));
// Generate a point out of plane
var point3 = point1 + 2.5f * trig.Normal;
// Check that it is not contained inside the triangle
Debug.Assert(!trig.Contains(point3));
// Generate a convex quadrilateral
var poly1 = new Vector3[] {
new Vector3(0f,0f,0f),
new Vector3(5f,0f,0f),
new Vector3(5f,3f,0f),
new Vector3(1f,7f,0f),
};
// Generate a non-convex quadrilateral
var poly2 = new Vector3[] {
new Vector3(0f,0f,0f),
new Vector3(5f,0f,0f),
new Vector3(2f,2f,0f),
new Vector3(1f,7f,0f),
};
// Check that it is convex
Debug.Assert(IsConvex(poly1));
// Check that it is not convex
Debug.Assert(!IsConvex(poly2));
}
Related
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);
}
Given a arbitrary reflection line how can you reflect a set of points with matrices? I have tried the following but i cannot get it to work:
Translate the system so that P1 of the reflectionline is in the
origin
Rotate the system so that the reflectionline is parallel to the Y
axis
Perform a Y axis reflection
Undo the rotation
Undo the translation
Iam trying to write a method to do this for me in C# basically i give it the 2 points of the line and i get the matrix back.
No rotations are needed since there is a formula for reflecting about any line through the origin. Let (a,b) and (c,d) be any two points on the reflection line. Let's say the point you want to reflect is (x,y).
Translate the coordinates so that (a,b) becomes the origin. Then (x,y) becomes (x-a,y-b). This step is just vector subtraction.
Reflect. This is where you need the matrix. You will multiply the matrix by the translated vector from step 1. Your result will be another vector (a 2-by-1 matrix).
Translate the coordinates back to the original system. This is the inverse of step 1, i.e., this is vector addition that just undoes the vector subtraction from step 1. In this step you're just adding (a,b) to your result from step 2.
The matrix for step 2 is:
H(θ) = [cos(2θ) sin(2θ)]
[sin(2θ) -cos(2θ)]
In the matrix, θ is the angle that the (translated) reflection line makes with the positive x-axis. As long as a and c are not equal, you can find θ by evaluating:
θ = arctangent( (d-b) / (c-a) )
You will get a value strictly between -π/2 and π/2. If a = c, that is, if the reflection line is vertical, then just take θ = π/2. Although if the reflection line is vertical (or horizontal, in which case θ = 0) then you can just use well-known reflection matrices for reflecting over the y- or x-axis.
This pretty much lays out the whole process of finding and using the matrix. It sounds like all you're asking for is finding the reflection matrix. I don't know C# well enough to use it in an answer, but here's pseudocode:
// (a,b) and (c,d) are any two distinct points on the reflection line
getMatrix(double a, double b, double c, double d)
double x11, x12, x21, x22; // Elements of the reflection matrix
if a == c // If the reflection line is vertical
x11 = -1; x12 = 0; x21 = 0; x22 = 1;
else if b == d // If the reflection line is horizontal
x11 = 1; x12 = 0; x21 = 0; x22 = -1;
else
double θ = arctangent( (d-b) / (c-a) );
x11 = cos(2 * θ);
x12 = sin(2 * θ);
x21 = x12; // sin(2 * θ) again
x22 = -x11; // -cos(2 * θ)
end if
return Matrix(x11, x12, x21, x22);
/* The above line returns a matrix with the following entries:
[ x11 x12 ]
[ x21 x22 ]
*/
And here's example pseudocode for using the above pseudocode:
// Reflect (x,y) over the line given by the points (a,b) and (c,d)
reflectPoint(double x, double y, double a, double b, double c, double d)
Matrix reflector = getMatrix(a, b, c, d);
Vector v1 = new Vector(x-a, x-b); // This is Step 1
Vector v2 = new Vector(a, b); // This is so we can do Step 3 later
return reflector * v1 + v2; // v1 already has Step 1 done
// reflector * v1 is Step 2
// + v2 is Step 3
There are more efficient ways of doing the above (like checking if one of the given points (a,b) and (c,d) is already the origin, for example) but the above should still work.
After fixing the typo i made here is what my code looks like:
private Transformer2D Reflect(Vector2D p1, Vector2D p2)
{
var translationMatrix = new TranslateTransformation2D(new Vector2D(0, 0) - p1);
var inverseTranslationMatrix = new TranslateTransformation2D(p1);
var translatedP2 = translationMatrix.Transform(p2); //What p2 would be if p1 of the line was translated to the origin.
var angleWithYaxis = new Vector2D(0, 1).AngleBetweenTwoVectors(translatedP2);
var rotationMatrix = new RotationTransformation2D(-angleWithYaxis * RhinoMath.Deg2Rad);
var inverseRotationMatrix = new RotationTransformation2D(angleWithYaxis * RhinoMath.Deg2Rad);
var reflectionMatrix = new ScaleTransformation2D(-1, 1);
return new Transformer2D(translationMatrix, rotationMatrix, reflectionMatrix, inverseRotationMatrix, inverseTranslationMatrix);
}
The classes iam using just abstract the matrices:
public class TranslateTransformation2D : MatrixTransformation2DBase
{
public TranslateTransformation2D(Vector2D translation)
{
TransformationMatrix = Matrix3x3.CreateTranslationMatrix(translation.X, translation.Y);
}
public TranslateTransformation2D(float x, float y)
{
TransformationMatrix = Matrix3x3.CreateTranslationMatrix(x, y);
}
}
This is how the Matrix3x3 class looks like:
public class Matrix3x3 : Matrix
{
public Matrix3x3(double m11, double m12, double m13,
double m21, double m22, double m23,
double m31, double m32, double m33)
{
Rows = 3;
Cols = 3;
Mat = new double[Rows * Cols];
Mat[0] = m11;
Mat[1] = m12;
Mat[2] = m13;
Mat[3] = m21;
Mat[4] = m22;
Mat[5] = m23;
Mat[6] = m31;
Mat[7] = m32;
Mat[8] = m33;
}
public static Matrix3x3 CreateTranslationMatrix(double x, double y)
{
return new Matrix3x3(1, 0, x,
0, 1, y,
0, 0, 1);
}
public static Matrix3x3 CreateScaleMatrix(double x, double y)
{
return new Matrix3x3(x, 0, 0,
0, y, 0,
0, 0, 1);
}
public static Matrix3x3 CreateIdentityMatrix()
{
return new Matrix3x3(1, 0, 0,
0, 1, 0,
0, 0, 1);
}
public static Matrix3x3 CreateRotationMatrix(double radians)
{
var cos = Math.Cos(radians);
var sin = Math.Sin(radians);
return new Matrix3x3(cos, -sin, 0,
sin, cos, 0,
0, 0, 1);
}
The Transformer2D class is special here since it simply combines all the transformations into one matrix so we only have to apply this matrix to get all our transformations:
public class Transformer2D : MatrixTransformation2DBase
{
public Transformer2D(params IMatrixTransformation2D[] transformations)
{
for (int i = transformations.Length - 1; i >= 0; i--)
{
var matrixTransformation2D = transformations[i];
if (TransformationMatrix != null)
{
TransformationMatrix = TransformationMatrix * matrixTransformation2D.TransformationMatrix;
}
else
{
TransformationMatrix = matrixTransformation2D.TransformationMatrix;
}
}
}
}
The MatrixTransformation2DBase class
public abstract class MatrixTransformation2DBase : IMatrixTransformation2D
{
public Matrix3x3 TransformationMatrix { get; protected set; }
public Vector2D Transform(Vector2D vector2Din)
{
return vector2Din*TransformationMatrix;
}
}
I could probably make it faster in a few places but the idea is that i dont have to worry anymore about the matrices themselves unless i want some new type of transformation.
For those wondering what matrix class i use internally its this one: https://github.com/darkdragon-001/LightweightMatrixCSharp
All i did was write some conveinence around this.
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);
}
I constructed a small function to check if a group of points are coplanar:
public static bool IsCoplanar(Point[] points)
{
// Ensure there are greater than three points (otherwise always coplanar)
if (points.Length < 4)
{
return true;
}
Point pointA = points[0];
Point pointB = points[1];
Point pointC = points[2];
// Calculate the scalar triple product using vectors formed from
// the first three points and each successive point to check that
// the point is on the same plane as the first three.
Vector vectorBA = pointB - pointA;
Vector vectorCA = pointC - pointA;
for (int i = 3; i < points.Length; i++)
{
Point pointD = points[i];
Vector vectorDA = pointD - pointA;
if (!(System.Math.Abs(vectorBA.Dot(vectorCA.Cross(vectorDA))) < Epsilon))
{
return false;
}
}
return true;
}
Unfortunately, it seems to be returning true in the case, for example, starting with 3 coplanar points:
(-50, 50, -50)
(-50, -50, -50)
(-50, -50, 50)
Which are fine. But if you add:
(50, -50, 50)
(50, -50, -50)
To the list and run again, it still returns true.
I've been looking at this for ages but haven't been able to spot the problem, does anyone have any idea?
Thanks.
Here is code in C#. Note, operations like Cross and Equal should be class operators, I did not do that. Also, I included edge case testing for things like coincidental points. I.E. what happens if input are non-unique points, like (50,50,50) followed by (50,50,50), your current code fails!
public static bool IsCoplanar(MyPoint[] points)
{
if (points.Length <= 3)
return true;
//input points may be the coincidental/same (edge case),
//so we first need to loop to find three unique points.
//the first unique point is by default at position 0,
//so we will start looking for second at position 1:
int unique_point2_index = 0;
int unique_point3_index = 0;
bool found_point2 = false;
bool found_point3 = false;
for (int i = 1; i < points.Length; ++i )
{
if (!found_point2)
{
if (!Equals(points[0], points[i]))
{
found_point2 = true;
unique_point2_index = i;
}
}
else if (!found_point3)
{
if (!Equals(points[0], points[i]) && !Equals(points[unique_point2_index], points[i]))
{
found_point3 = true;
unique_point3_index = i;
}
}
else
break;
}
//if we did not find three unique points, then all of the points are coplanar!
if (!found_point3)
return true;
//we found three unique points lets loop through the rest and check if those
//are also coplanar. We do that as following:
//First compute the plane normal:
MyPoint P1 = points[0];
MyPoint P2 = points[unique_point2_index];
MyPoint P3 = points[unique_point3_index];
MyPoint vecP1P2 = Minus(P2, P1); //Should be class operator, P2 - P1
MyPoint vecP1P3 = Minus(P3, P1);
MyPoint normal = Cross(vecP1P2, vecP1P3);
//Secondly, for the remainder of points, we compute
//a vector from P1 to each point,
//and take the dot product with the normal.
//This should be zero (+- epsilon) for coplanar points
for (int i = unique_point3_index + 1; i < points.Length; ++i)
{
MyPoint testVec = Minus(points[i], P1);
double dot = Dot(testVec, normal);
//include error boundary for double precision
if (Math.Abs(dot) > 0.000001)
return false;
}
return true;
}
Nothing obvious about the code jumps out at me, but you might try a slightly different approach.
Given the formula for a plane:
Ax + By + Cz + D = 0
take your first three points, which define a plane, and generate the coefficients A, B, C and D.
For the rest of the points, then check:
Point v;
float d = A * v.x + B * v.y + C * v.d;
d is now the distance from that point to the plane, along the plane's normal.
If d is less than D (the distance of the plane to the origin along its normal), the point is behind the plane (ie, opposite side of the plane from which the normal is pointing). If d is greater than D, the point is in front of the plane.
if abs(d - D) < float.Epsilon), then the point may safely be assumed to lie in the plane.
Example (from this site)
Given points P, Q, R in space, find the equation of the plane through the 3 points.
If P = (1, 1, 1), Q = (1, 2, 0), R = (-1, 2, 1).
We seek the coefficients of an equation ax + by + cz = d, where P, Q and R satisfy the equations, thus:
a + b + c = d
a + 2b + 0c = d
-a + 2b + c = d
Subtracting the first equation from the second and then adding the first equation to the third, we eliminate a to get
b - c = 0
4b + c = 2d
Adding the equations gives 5b = 2d, or b = (2/5)d, then solving for c = b = (2/5)d and then a = d - b - c = (1/5)d.
So the equation (with a nonzero constant left in to choose) is d(1/5)x + d(2/5)y + d(2/5)z = d, so one choice of constant gives
x + 2y + 2z = 5
or, A = 1, B = 2, C = 2, and D = -5.
Once you have those, checking the rest of the points is simply substituting the point's x,y,z into the plane equation, and comparing the output to the D distance of the plane to the origin.
Note that the coefficients can also be found using a matrix to solve a system of three equations with three unknowns. If you already have a matrix class available, it should be pretty straightforward to use it to find the coefficients.
I have a binary object in a 3d array located in the origin (0,0,0). I need rotate this object dinamically in z axis. How can I rotate that multidimensional array with no fixed size in any angle?
I have created a 3d Point Class:
public class Point3
{
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
}
I'm thinking in do a foreach in each point and rotate it:
foreach (Point3 p in listPoint3)
{
RotatePoint(p, angle);
}
Any suggestion?
You colud create a routine to rotate each point using the parametric equation for rotate the 3d object.
x' = x*cos(o)-y*sin(o)
y' = y*sin(o)-y*cos(o)
z' = z
private Point3 RotatePoint(Point3 p0, int angle)
{
Point3 p = new Point3()
{
X = p0.X * Math.Cos(angle) - p0.Y * Math.Sin(angle),
Y = p0.X * Math.Sin(angle) + p0.Y * Math.Cos(angle),
Z = p0.Z,
};
return p;
}
You need to know what axis you want to rotate on. But if this is only a question where to look at. (namespace System.Windows.Media.Media3D)
You can try this:
double angle = 45;
RotateTransform3D zrotation = new RotateTransform3D(new AxisAngleRotation3D(
new Vector3D(0, 0, 1), angle));
foreach (Point3D p in listPoint3)
{
Point3D rotatedPoint = zrotation.Transform(p);
}
You should use the build-in Point3D
Also if you want to Stack those: (multiple transforms)
double zAngle = 45;
double xAngle = 10;
Transform3DGroup group = new Transform3DGroup();
group.Children.Add( new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(1, 0, 0), xAngle)));
group.Children.Add( new RotateTransform3D(new AxisAngleRotation3D(new Vector3D(0, 0, 1), zAngle)));
foreach (Point3D p in listPoint3)
{
Point3D rotatedPoint = group.Transform(p);
}
So you have a "monochrome object" which is stored like a 3D bitmap which you want to rotate around the Z axis. You must first understand that after a number of rotations you will end up with optical aberrations caused by the fact that you're using an array index which is a natural number to represent a coordinate of an object's component.
Upon rotation, any integer value will most likely become an irrational number. The fact that traditionally (not talking about special programs and frameworks) people store approximations of irrational numbers in double or float or decimal variables (which can only store a small subset of the rational numbers set) is nothing compared to the approximation of an irrational number by storing it in an integer (an array index).
Furthermore, even if that quality loss if of no big importance in the case of your application, you must be sure you understand that mathematically speaking, after a number of rotations, your 3d shape will be trimmed by the cylinder which is inscribed in the original parallelepiped, alongside the Z axis.
It goes like this. You said you already made a class called Point3:
public class Point3 {
public double X { get; set; }
public double Y { get; set; }
public double Z { get; set; }
}
Maybe you should follow #Jeroen van Langen's advice and use a standard class, if such a class already exists. The benefit would be that if someone out there already built or will ever build a library which uses that class you can start using the library right away.
But that's not so important right now.
#Alpert already gave a great C# code for rotating a point around the oZ axis. This is n "extension method" adaption of that code:
public static class RotationHelpers {
public static Point3 RotatePoint(this Point3 point, int angle) {
var result = new Point3() {
X = point.X * Math.Cos(angle) - point.Y * Math.Sin(angle),
Y = point.X * Math.Sin(angle) + point.Y * Math.Cos(angle),
Z = point.Z,
};
return result;
}
...
}
You can go even further and make an extension method which rotates a sequence of points around the oZ axis:
public static class RotationHelpers {
...
public static IEnumerable<Point3> RotatePoints(this IEnumerable<Point3> points, int angle) {
foreach (var point in points)
yield return point.RotatePoint(angle);
}
...
}
Now you said you have a 3d primitive matrix with 1s and 0s in it:
int[,,] matrix;
You need to somehow convert the intrinsically defined points in that matrix into a sequence of Point3 instances, rotate those and then convert the resulting sequence back into an int[,,] matrix.
That could be achieved like so (remember about the loss of quality I was talking about earlier):
public static class RotationHelpers {
...
public static IEnumerable<Point3> ToPoints(this int[,,] matrix) {
int lx = matrix.GetLength(0);
int ly = matrix.GetLength(1);
int lz = matrix.GetLength(2);
for (int x = 0; x < lx; x++)
for (int y = 0; y < ly; y++)
for (int z = 0; z < lz; z++) {
bool is1 = matrix[x, y, z] != 0;
if (is1)
yield return new Point3 {
X = x - lx / 2,
Y = y - ly / 2,
Z = z - lz / 2
};
}
}
...
}
That will take all of the cells in the WIDTH x HEIGHT x DEPTH matrix and for every cell which is not equal to 0 it will yield a new Point3 instance with the coordinates of that particular position.
That sequence can then be rotate by an angle using the earlier described RotatePoints method and then the following method could be used to "render" back the resulting sequence of Point3 instances into the array:
public static class RotationHelpers {
...
public static void AssignPoints(this int[,,] matrix, IEnumerable<Point3> points) {
int lx = matrix.GetLength(0);
int ly = matrix.GetLength(1);
int lz = matrix.GetLength(2);
for (int x = 0; x < lx; x++)
for (int y = 0; y < ly; y++)
for (int z = 0; z < lz; z++)
matrix[x, y, z] = 0;
foreach (var point in points) {
// this is when quality is lost, because things like 1.7 and 1.71
// will both become =2
var x = (int)Math.Round(point.X) + lx / 2;
var y = (int)Math.Round(point.Y) + ly / 2;
var z = (int)Math.Round(point.Z) + lz / 2;
// this is where you loose parts of the object because
// it doesn't fit anymore inside the parallelepiped
if ((x >= 0) && (y >= 0) && (z >= 0) &&
(x < lx) && (y < ly) && (z < lz))
matrix[x, y, z] = 1;
}
}
...
}
To wrap it up, you can use all of these methods like so:
int[,,] matrix = ...
int angle = ...
IEnumerable<Point3> points = matrix.ToPoints();
IEnumerable<Point3> rotatedPoints = points.RotatePoints(angle);
matrix.AssignPoints(rotatedPoints);
// now you have the original matrix, rotated by angle