Working on 3d tube extrusion along a Bezier path in 3d space using C# and have come to an impasse. I have integrated GLMSharp for vectors, dot, cross and normal etc.. I'm unsure how to create an Up Vector and whether that's the problem. I am outputting to .3ds format and the following image shows gaps or twists in some cases. I am inquiring for a solution.
// frenet serret code
void get_circle(int i, double theta, float[,] mmp, float[] p1, float[] p2, float rad,float[] cen)
{
float[] n = new float[3];
vec3 tp1 = new vec3(p1[0], p1[1], p1[2]);
vec3 tp2 = new vec3(p2[0], p2[1], p2[2]);
vec3 T = (tp2 - tp1);
T = T.Normalized;
vec3 B = vec3.Cross(T, tp2 + tp1);
B = B.Normalized;
vec3 N = vec3.Cross(B, T);
N = N.Normalized;
float x = (float)Math.Cos(theta)*rad;
float y = (float)Math.Sin(theta)*rad;
vec3 vertex = tp1 + B * x + N * y;
mmp[i,0] = cen[0] + vertex.x;
mmp[i,1] = cen[1] + vertex.y;
mmp[i,2] = cen[2] + vertex.z;
}
twists or gaps
As far as I understand, p1 and p2 are points, so tp2 + tp1 defines some point (vector) that has no relevance to binormal B. When this vector is collinear with tangent T, you have got zero B adn gap.
To calculate Frenel trihedron in some point of path, you need at least three consequtive points of that path - let's name them Prev, Curr, Next.
Binormal B might be calculated as (except for case of three collinear points - here binormal is not defined, and one might use previous value if possible)
B = Cross(Next-Curr, Curr - Prev)
Note that more reliable value of tangent also might account for neighbor points - so instead of
T = Next - Curr
you can use (when spacing is rather uniform) (compare here)
T = Next - Prev
MBo, Thanx for the expedited response and the routine based on your advise is as follows:
void cap_middle(int i, double theta, float[,] mmp, vec3 prev, vec3 curr, vec3 next,float rad,float[] cen)
{
vec3 T = (curr - next);
T = T.Normalized;
vec3 B = vec3.Cross(next - curr, curr - prev);
B = B.Normalized;
vec3 N = vec3.Cross(B, T);
N = N.Normalized;
float x = (float)Math.Cos(theta)*rad;
float y = (float)Math.Sin(theta)*rad;
vec3 vertex = curr + B * x + N * y;
mmp[i,0] = cen[0] + vertex.x;
mmp[i,1] = cen[1] + vertex.y;
mmp[i,2] = cen[2] + vertex.z;
}
Although, another problem cropped up, the connecting cylinders from each Bezier path connected seems to have a winding order problem as shown in included graphic. The vertices are not consistent. Any Ideas?
winding order
Related
I wrote an application in C# which controls a camera through a 3D environment.
At the moment the camera follows a path which is defined as an array of points, where each point is defined as: x, y, z & rotation for each axis (everything is a float)
The camera position between those points are computed by linear interpolation every frame.
Because this leads to very clunky movement, I want to implement cubic spline interpolation instead.
There are many different algorithms, but so far I set my eyes on the following, since those feel "more correct", which means they don't overshoot:
Piecewise Cubic Hermite Interpolating Polynomial (PCHIP)
Modified Akima piecewise cubic Hermite interpolation (MAKIMA)
My biggest problem is, that I am absolutely not a mathematician and I can't read and understand the gibberish found online for their definitions, so I need help translating it into code.
Constraints for the implementation
Algorithm should use either PCHIP or MAKIMA (I am open for other "correct" approaches though)
Interpolated values must be a function of time (t from 0 to {points.Length})
Must run for 3D points, not only 2D
Should not contain any external libraries if possible
Libraries
I have checked the following, but they either don't support 3D points, or interpolation by time (and aren't intuitive to use either... not even talking about the license here):
Math.NET Numerics
ALGLIB
Here is a small code stump which I came up with to demonstrate what I am looking for:
static float[,] points = new float[,] {
{10, 10, 10},
{35, 5, 25},
{15, 30, 15}
};
static int numberOfPoints = points.Length / 3; // dividing for each stored float value per point: only x, y, z for simplification
static void Main()
{
List<float[]> pointsWithInterpolation = new(); // holding both known and interpolated points
float stepSize = 0.1f; // step size for t
for (int i = 0; i < numberOfPoints - 1; i++)
{ // loop for each spline segment
pointsWithInterpolation.Add(new float[] { points[i, 0], points[i, 1], points[i, 2] }); // add an allready known point to the list
for (float t = stepSize; t < 1f; t += stepSize)
{ // should run (1 / {stepSize} - 2) times
float[] interpolatedPoint = Interpolate(i + t);
pointsWithInterpolation.Add(interpolatedPoint); // add an interpolated point to the list
}
if (i == numberOfPoints - 1)
{ // add the very last point if we have finished the last segment
pointsWithInterpolation.Add(new float[] { points[i + 1, 0], points[i + 1, 1], points[i + 1, 2] }); // add an allready known point to the list
}
}
}
private static float[] Interpolate(float t)
{
// point indexes
int p1 = (int)t;
int p2 = p1 + 1;
int p0 = p1 == 0 ? 0 : p1 - 1; // if there are no previous point, set it to the first one
int p3 = p2 == numberOfPoints - 1 ? p2 : p2 + 1; // if there are no following points, set it to the last one
float x0 = points[p0, 0];
float x1 = points[p1, 0];
float x2 = points[p2, 0];
float x3 = points[p3, 0];
float y0 = points[p0, 1];
float y1 = points[p1, 1];
float y2 = points[p2, 1];
float y3 = points[p3, 1];
float z0 = points[p0, 2];
float z1 = points[p1, 2];
float z2 = points[p2, 2];
float z3 = points[p3, 2];
float x, y, z;
/* black magic hocus pocus happening here
x = ???;
y = ???;
z = ???;
*/
float[] point = new float[] { x, y, z };
return point;
}
So the question is: what is the formula for solving for x/y/z (the formula should be the same for each variable, but they use different variables).
Yes I know, this results in a constant number of interpolated points between the "real" ones, even if the distance of segments widely varies - I will tackle the issue of constant speed separately.
If I am not mistaken, every interpolation between p1 and p2 needs 4 values, so I set p0 equal to p1 for the very first segment and p3 equal to p2 for the last one to make up for non-existant points.
I haven't performance optimized the code, so I (hopefully) make it more clear/easy to understand what I try to do.
Of course I would also be thankful for code in other languages, as long as it fulfils the implementation constraints and doesn't use functions which aren't available in C# (and also no assembler please lmao)
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);
}
I have two rays. Each ray has a start location vector (Vector3D) and a direction vector (Vector3D), but continue on to infinity. They are both on the same plane, but in a 3D environment. The Rays are interdependent, which means that they might not mirror each other perfectly. From this i need to calculate the location at which these rays intersect in the 3D environment and output it as a vector. In essence: a rangefinder.
How should i go about doing this? Is there a better way of doing it than using the C# Ray structure, is it even possible?
I am a pretty new coder (read: bad) but any answer is appreciated, I would enjoy it if an explanation was included.
Crude image of the rays
Two lines in 3D space only intersect if they are on the same plane. The probability that two random lines in space intersect is really small.
When you want to find out if two rays intersect, if you are looking for an exact intersection point, the chance is that you are not going to be able to calculate it due to floating point errors.
The next best thing is to find the shortest distance between two rays. Then if that distance is smaller than a certain threshold (defined by you) we could say the rays are intersecting.
Finding shortest distance
Here are two rays in 3D space, with the blue vector representing the shortest distance.
Let's take a frame from that gif:
Legend:
p1 is ray1.Position
p2 is ray2.Position
d1 is ray1.Direction
d2 is ray2.Direction
d3 is the cross product d1 x d2
The cross product of the rays directions will be perpendicular to both rays, so it's the shortest direction from ray to ray. If lines are parallel, the cross product will be zero, but for now lets only deal with non-parallel lines.
From the photo, we get the equation:
p1 + a*d1 + c*d3 = p2 + b*d2
Rearranged so the variables are on the left:
a*d1 - b*d2 + c*d3 = p2 - p1
Since each of the know values (d1, d2, d3, p1 and p2) has three components (x,y,z), this is a system of three linear equations with 3 variables.
a*d1.X - b*d2.X + c*d3.X = p2.X - p1.X
a*d1.Y - b*d2.Y + c*d3.Y = p2.Y - p1.Y
a*d1.Z - b*d2.Z + c*d3.Z = p2.Z - p1.Z
Using Gaussian elimination, we get the values of a, b and c.
If both a and b are positive, the position of the intersection will be
Vector3 position = ray1.Position + a*ray1.Direction;
Vector3 direction = c * d3; //direction.Length() is the distance
You could return these values as a Ray for convenience.
If either a or b are negative, this means that the calculated shortest distance would be behind one (or both) of the rays, so a different method should be used to find the shortest distance. This method is the same for lines that are parallel (cross product d1 x d2 is zero).
Now the calculation becomes finding which ray (positive direction) is closest to the position (p1 or p2) of the other ray. For this we use dot product (projection of a vector onto another vector)
Legend:
dP = p2 - p1
Before calculating dot product of d1 dot dP, make sure that d1 (or d2) are normalized (Vector3.Normalize()) - dot product is meant to work on unit vectors.
Now it's a matter of finding which is the shortest distance, based on the projection factor (result of dot) on ray1 (lets call it a2) and projection factor on ray2 (lets call it b2).
If both a2 and b2 are negative (negative side of the ray), then the shortest distance is from position to position. If one is in the negative direction then the other one is the shortest. Else it's the shorter of the two.
Working code:
public Ray FindShortestDistance(Ray ray1, Ray ray2)
{
if (ray1.Position == ray2.Position) // same position - that is the point of intersection
return new Ray(ray1.Position, Vector3.Zero);
var d3 = Vector3.Cross(ray1.Direction, ray2.Direction);
if (d3 != Vector3.Zero) // lines askew (non - parallel)
{
//d3 is a cross product of ray1.Direction (d1) and ray2.Direction(d2)
// that means d3 is perpendicular to both d1 and d2 (since it's not zero - we checked that)
//
//If we would look at our lines from the direction where they seem parallel
// (such projection must always exist for lines that are askew)
// we would see something like this
//
// p1 a*d1
// +----------->x------
// |
// | c*d3
// p2 b*d2 v
// +------->x----
//
//p1 and p2 are positions ray1.Position and ray2.Position - x marks the points of intersection.
// a, b and c are factors we multiply the direction vectors with (d1, d2, d3)
//
//From the illustration we can the shortest distance equation
// p1 + a*d1 + c*d3 = p2 + b*d2
//
//If we rearrange it so we have a b and c on the left:
// a*d1 - b*d2 + c*d3 = p2 - p1
//
//And since all of the know variables (d1, d2, d3, p2 and p1) have 3 coordinates (x,y,z)
// now we have a set of 3 linear equations with 3 variables.
//
// a * d1.X - b * d2.X + c * d3.X = p2.X - p1.X
// a * d1.Y - b * d2.Y + c * d3.Y = p2.Y - p1.Y
// a * d1.Z - b * d2.Z + c * d3.Z = p2.Z - p1.Z
//
//If we use matrices, it would be
// [d1.X -d2.X d3.X ] [ a ] [p2.X - p1.X]
// [d1.Y -d2.Y d3.Y ] * [ a ] = [p2.Y - p1.Y]
// [d1.Z -d2.Z d3.Z ] [ a ] [p2.Z - p1.Z]
//
//Or in short notation
//
// [d1.X -d2.X d3.X | p2.X - p1.X]
// [d1.Y -d2.Y d3.Y | p2.Y - p1.Y]
// [d1.Z -d2.Z d3.Z | p2.Z - p1.Z]
//
//After Gaussian elimination, the last column will contain values a b and c
float[] matrix = new float[12];
matrix[0] = ray1.Direction.X;
matrix[1] = -ray2.Direction.X;
matrix[2] = d3.X;
matrix[3] = ray2.Position.X - ray1.Position.X;
matrix[4] = ray1.Direction.Y;
matrix[5] = -ray2.Direction.Y;
matrix[6] = d3.Y;
matrix[7] = ray2.Position.Y - ray1.Position.Y;
matrix[8] = ray1.Direction.Z;
matrix[9] = -ray2.Direction.Z;
matrix[10] = d3.Z;
matrix[11] = ray2.Position.Z - ray1.Position.Z;
var result = Solve(matrix, 3, 4);
float a = result[3];
float b = result[7];
float c = result[11];
if (a >= 0 && b >= 0) // normal shortest distance (between positive parts of the ray)
{
Vector3 position = ray1.Position + a * ray1.Direction;
Vector3 direction = d3 * c;
return new Ray(position, direction);
}
//else will fall through below:
// the shortest distance was between a negative part of a ray (or both rays)
// this means the shortest distance is between one of the ray positions and another ray
// (or between the two positions)
}
//We're looking for the distance between a point and a ray, so we use dot products now
//Projecting the difference between positions (dP) onto the direction vectors will
// give us the position of the shortest distance ray.
//The magnitude of the shortest distance ray is the the difference between its
// position and the other rays position
ray1.Direction.Normalize(); //needed for dot product - it works with unit vectors
ray2.Direction.Normalize();
Vector3 dP = ray2.Position - ray1.Position;
//shortest distance ray position would be ray1.Position + a2 * ray1.Direction
// or ray2.Position + b2 * ray2.Direction (if b2 < a2)
// or just distance between points if both (a and b) < 0
//if either a or b (but not both) are negative, then the shortest is with the other one
float a2 = Vector3.Dot(ray1.Direction, dP);
float b2 = Vector3.Dot(ray2.Direction, -dP);
if (a2 < 0 && b2 < 0)
return new Ray(ray1.Position, dP);
Vector3 p3a = ray1.Position + a2 * ray1.Direction;
Vector3 d3a = ray2.Position - p3a;
Vector3 p3b = ray1.Position;
Vector3 d3b = ray2.Position + b2 * ray2.Direction - p3b;
if (b2 < 0)
return new Ray(p3a, d3a);
if (a2 < 0)
return new Ray(p3b, d3b);
if (d3a.Length() <= d3b.Length())
return new Ray(p3a, d3a);
return new Ray(p3b, d3b);
}
//Solves a set of linear equations using Gaussian elimination
float[] Solve(float[] matrix, int rows, int cols)
{
for (int i = 0; i < cols - 1; i++)
for (int j = i; j < rows; j++)
if (matrix[i + j * cols] != 0)
{
if (i != j)
for (int k = i; k < cols; k++)
{
float temp = matrix[k + j * cols];
matrix[k + j * cols] = matrix[k + i * cols];
matrix[k + i * cols] = temp;
}
j = i;
for (int v = 0; v < rows; v++)
if (v == j)
continue;
else
{
float factor = matrix[i + v * cols] / matrix[i + j * cols];
matrix[i + v * cols] = 0;
for (int u = i + 1; u < cols; u++)
{
matrix[u + v * cols] -= factor * matrix[u + j * cols];
matrix[u + j * cols] /= matrix[i + j * cols];
}
matrix[i + j * cols] = 1;
}
break;
}
return matrix;
}
It is so simple. you need to apply AAS Triangle formula.
Consider the above figure as triangle,
Ray1 location as A and Ray2 Location as B and you need to find point c. Using dot product find angle between Ray AB and AC (alpha), BA and BC (theta).
Find the distance between location A and B (D). So finally you have alpha, beta and D, i.e (2 angles and 1 side of the triangle.) Apply AAS method and find C location.
https://www.mathsisfun.com/algebra/trig-solving-aas-triangles.html.
There are two Point3D's (A and B) and I want to calculate the points of a cuboid (a,b,c ... h) surrounding the line between A and B like a hull:
There is one degree of freedom, the angle of the cuboid, because it can rotate around the line AB. I am not sure yet if this is a problem.
I tried to calculate a vector normal to AB, D, and then the cross product of AB ⨯ AD = E. In the code, C is A - B so its the offset parallel to AB.
I normalized these three vectors (C, D and E) and multiplied it with an offset to add / subtract them from A and B. It's not quite working yet.
EDIT: see ja72's code for solution
i also implemented a way of finding a normal vector:
double ax = Vector3D.AngleBetween(E, new Vector3D(1, 0, 0));
double ay = Vector3D.AngleBetween(E, new Vector3D(0, 1, 0));
double az = Vector3D.AngleBetween(E, new Vector3D(0, 0, 1));
ax = Math.Abs(ax - 90);
ay = Math.Abs(ay - 90);
az = Math.Abs(az - 90);
if (ax <= ay & ax <= az)
{
n = Vector3D.CrossProduct(E, new Vector3D(1, 0, 0));
}
else if (az <= ax && az <= ay)
{
n = Vector3D.CrossProduct(E, new Vector3D(0, 0, 1));
}
else
{
n = Vector3D.CrossProduct(E, new Vector3D(0, 1, 0));
}
n = normalize(n);
You need two direction vectors. One is along the line AB given by
Vector3D e = Normalize(B-A)
and one to descibe the "up" direction for the cross section. This can be given, or it can be calculated with the following algorithm (with preference towards +y)
if( e.X != 0 || e.Z != 0 )
{
// choose direction perpendicular to line closest to +y direction
Vector3D n = [-e.X*e.Y, e.X*e.X+e.Z*e.Z, -e.Z*e.Y];
} else {
// if line along +y already then choose +z for up vector
Vector3D n = [ 0, 0, 1];
}
Now you can calculate the 3rd direction to form a coordinate system
Vector3D k = Normalize( Cross(e,n) )
And you assemble the 3×3 rotation matrix that transforms local coordinates to world coordinates with
| k.X n.X e.X |
R = | k.Y n.Y e.Y |
| k.Z n.Z e.Z |
The local coordinate have the direction along the line as +z such that
Point3D a = A + R*[w,h,0]
Point3D b = A + R*[-w,h,0]
Point3D c = A + R*[w,-h,0]
Point3D d = A + R*[-w,-h,0]
Point3D e = B + R*[w,h,0]
Point3D f = B + R*[-w,h,0]
Point3D g = B + R*[w,-h,0]
Point3D h = B + R*[-w,-h,0]
where R*[x,y,z] designates a matrix-vector multiplication, w and h are the width and height of the rectangular cross section, and A, B are the point A and B position vectors. Addition here between vectors is element by element.
I have checked the code in my own code and it works. vec3 is alias for a 3D vector, mat3 is alias for 3×3 matrix.
vec3 u=(B-A).Normalized();
vec3 n = vec3.O;
if(Math.Abs(u.X)<=Math.Abs(u.Y)&&Math.Abs(u.X)<=Math.Abs(u.Z))
{
n=new vec3(u.Y*u.Y+u.Z*u.Z, -u.Y*u.X, -u.Z*u.X);
}
else if(Math.Abs(u.Y)<=Math.Abs(u.X)&&Math.Abs(u.Y)<=Math.Abs(u.Z))
{
n=new vec3(-u.X*u.Y, u.X*u.X+u.Z*u.Z, -u.Z*u.Y);
}
else if(Math.Abs(u.Z)<=Math.Abs(u.X)&&Math.Abs(u.Z)<=Math.Abs(u.Y))
{
n=new vec3(-u.X*u.Z, -u.Y*u.Z, u.X*u.X+u.Y*u.Y);
}
vec3 v=n.Cross(u);
mat3 R=mat3.Combine(v, n, u);
var a=A+R*new vec3(wt, ht, 0);
var b=A+R*new vec3(-wt, ht, 0);
var c=A+R*new vec3(wt, -ht, 0);
var d=A+R*new vec3(-wt, -ht, 0);
var e=B+R*new vec3(wt, ht, 0);
var f=B+R*new vec3(-wt, ht, 0);
var g=B+R*new vec3(wt, -ht, 0);
var h=B+R*new vec3(-wt, -ht, 0);
I can't give you a real piece of code, but I can give you an idea.
Ok, suppose you are already have a proper cuboid. I mean it has right width and height.
Lets locate it on plane xy. After that you need to offset it to center (minus offset vector). The last thing is to rotate it according to your line rotation.
Again:
Create cuboid and locate it on xy plane
Move it according to your offsets
Rotate it according to your line rotation
You may use matrix multiplication to achieve this transformations.
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.