I have 2 function to either calculate a point on a spline, quadratic or cubic:
struct vec2 {float x, y;};
vec2 spline_quadratic(vec2 & a, vec2 & b, vec2 & c, float t) {
return {
(1 - t) * (1 - t) * p1.x + 2 * (1 - t) * t * p2.x + t * t * p3.x,
(1 - t) * (1 - t) * p1.y + 2 * (1 - t) * t * p2.y + t * t * p3.y
};
}
vec2 spline_cubic(vec2 & a, vec2 & b, vec2 & c, vec2 & d, float t){
return {
//B(t) = (1-t)**3 p0 + 3(1 - t)**2 t P1 + 3(1-t)t**2 P2 + t**3 P3
(1 - t) * (1 - t) * (1 - t) * p1.x + 3 * (1 - t) * (1 - t) * t * p2.x + 3 * (1 - t) * t * t * p3.x + t * t * t * p4.x,
(1 - t) * (1 - t) * (1 - t) * p1.y + 3 * (1 - t) * (1 - t) * t * p2.y + 3 * (1 - t) * t * t * p3.y + t * t * t * p4.y
};
Is it possible to join several curves of an array of points?
I'm looking to make a function that has this signature:
vector<vec2> spline_join(vector<vec2> & points, int segments = 16){
vector<vec2> spline_points;
for(int i = 0; i < points.size()-2; ++i){
for(int div = 0; div < segments; ++div){
spline_points.push_back(spline_quadratic(points[0], points[1], points[2], 1.f/segments);
}
}
}
I've read that it requires interpolation, but I'm not sure... What would the code look like? I've searched and I can't find relevant question and answers...
I've seen there are libraries, but I'm looking for a shorter implementation.
Edit: I've tried the question and answer here and apparently this is what I want:
Joining B-Spline segments in OpenGL / C++
The code is not really clean but after some cleaning, it does work.
I've cleaned this answer Joining B-Spline segments in OpenGL / C++
This is not an Hermite spline, an hermite spline passes through the points, a B-spline does not.
Here is what worked and the result
float B0(float u) {
//return float(pow(u - 1, 3) / 6.0);
// (1-t)*(1-t)*(1-t)/6.f
return float(pow(1-u, 3) / 6.0);
}
float B1(float u) {
return float((3 * pow(u, 3) - 6 * pow(u, 2) + 4) / 6.0);
// (3 * t * t * t - 6 * t * t + 4) / 6
}
float B2(float u) {
return float((-3 * pow(u, 3) + 3 * pow(u, 2) + 3 * u + 1) / 6.0);
// (-3 * t * t * t + 3 * t * t + 3 * t + 1) / 6
}
float B3(float u) {
return float(pow(u, 3) / 6.0);
// t * t * t / 6
}
vector<Vec2> computeBSpline(vector<Vec2>& points) {
vector<Vec2> result;
int MAX_STEPS = 100;
int NUM_OF_POINTS = points.size();
for (int i = 0; i < NUM_OF_POINTS - 3; i++)
{
//cout << "Computing for P" << i << " P " << i + 1 << " P " << i + 2 << " P " << i + 3 << endl;
for (int j = 0; j <= MAX_STEPS; j++)
{
float u = float(j) / float(MAX_STEPS);
float Qx =
B0(u) * points[i].x
+ B1(u) * points[i + 1].x
+ B2(u) * points[i + 2].x
+ B3(u) * points[i + 3].x;
float Qy =
B0(u) * points[i].y
+ B1(u) * points[i + 1].y
+ B2(u) * points[i + 2].y
+ B3(u) * points[i + 3].y;
result.push_back({ Qx, Qy });
//cout << count << '(' << Qx << ", " << Qy << ")\n";
}
}
return result;
}
Let's say AB1, AB2, CD1, CD2. AB1&AB2 and CD1&CD2 3D Points makes a Line Segment. And the Said Line segments are Not in the same Plane.
AP is a point Line segment AB1&AB2,
BP is a point Line segment CD1&CD2.
Point1 and Point2 Closest To each other (Shortest distance between the two line segment)
Now, how can I Find the said two points Point1 and Point2? What method should I use?
Below is only partially solved For full solution please See this answer here... because This function does not work when Two Line is on the same plane...
Thanks to #MBo I have come across Geometry GoldMine of Code and Explanations! They have Many Source Code Contributors! i picked one from there here it is clean and great!
bool CalculateLineLineIntersection(Vector3D p1, Vector3D p2, Vector3D p3, Vector3D p4, Vector3D& resultSegmentPoint1, Vector3D& resultSegmentPoint2)
{
// Algorithm is ported from the C algorithm of
// Paul Bourke at http://local.wasp.uwa.edu.au/~pbourke/geometry/lineline3d/
resultSegmentPoint1 = { 0,0,0 };
resultSegmentPoint2 = { 0,0,0 };
Vector3D p13 = VectorMinus(p1, p3);
Vector3D p43 = VectorMinus(p4, p3);
/*if (p43.LengthSq() < Math.Epsilon) {
return false;
}*/
Vector3D p21 = VectorMinus(p2, p1);
/*if (p21.LengthSq() < Math.Epsilon) {
return false;
}*/
double d1343 = p13.x * (double)p43.x + (double)p13.y * p43.y + (double)p13.z * p43.z;
double d4321 = p43.x * (double)p21.x + (double)p43.y * p21.y + (double)p43.z * p21.z;
double d1321 = p13.x * (double)p21.x + (double)p13.y * p21.y + (double)p13.z * p21.z;
double d4343 = p43.x * (double)p43.x + (double)p43.y * p43.y + (double)p43.z * p43.z;
double d2121 = p21.x * (double)p21.x + (double)p21.y * p21.y + (double)p21.z * p21.z;
double denom = d2121 * d4343 - d4321 * d4321;
/*if (Math.Abs(denom) < Math.Epsilon) {
return false;
}*/
double numer = d1343 * d4321 - d1321 * d4343;
double mua = numer / denom;
double mub = (d1343 + d4321 * (mua)) / d4343;
resultSegmentPoint1.x = (float)(p1.x + mua * p21.x);
resultSegmentPoint1.y = (float)(p1.y + mua * p21.y);
resultSegmentPoint1.z = (float)(p1.z + mua * p21.z);
resultSegmentPoint2.x = (float)(p3.x + mub * p43.x);
resultSegmentPoint2.y = (float)(p3.y + mub * p43.y);
resultSegmentPoint2.z = (float)(p3.z + mub * p43.z);
return true;
}
So Far I have Tried All these Below which works only when both Line segments have the same Magnitude...
Link 1
Link 2
I tried Calculating the centroid of both line segments and calculating the nearest Point on Segment From the midpoint. (I know how to calculate the Closest Point line segment from another Point)
But This only works when Both Line segments are of equal length AND each of Both the Linesegment's MidPoint is perpendicular to Each other and the centroid...
NOTE:Visual Geometry Geogbra3D for a visual representation of these Points
NOTE:AB1CD means From Point AB1 to Line CD(not segment)
AB1 = (6.550000, -7.540000, 0.000000 )
AB2 = (4.540000, -3.870000, 6.000000 )
CD1 = (0.000000, 8.000000, 3.530000 )
CD2 = (0.030000, -7.240000, -1.340000 )
PointCD1AB = (3.117523, -1.272742, 10.246199 )
PointCD2AB = (6.318374, -7.117081, 0.691420 )
PointAB1CD = (0.029794, -7.135321, -1.306549 )
PointAB2CD = (0.019807, -2.062110, 0.314614 )
Magntidue of PointCD1AB - P1LineSegmentCD = 11.866340
Magntidue of PointCD2AB - P2LineSegmentCD = 6.609495
Magntidue of PointAB1CD - P1LineSegmentAB = 6.662127
Magntidue of PointAB2CD - P2LineSegmentAB = 9.186399
Magntidue of PointCD1AB - PointAB1CD = 13.318028
Magntidue of PointCD2AB - PointAB2CD = 8.084965
Magntidue of PointCD1AB - PointAB2CD = 10.433375
Magntidue of PointCD2AB - PointAB1CD = 6.598368
Actual Shortest Point are
Point1 = (0.01, 1.59, 1.48 )
Point2 = (-1.23, 1.11, 3.13 )
Magnitude of Point1 And Point2 = 2.1190799890518526
For the Above Data, I used this Below Function
void NearestPointBetweenTwoLineSegmentOfVariedLength(Vector3D P1LineSegmentAB, Vector3D P2LineSegmentAB, Vector3D P1LineSegmentCD, Vector3D P2LineSegmentCD, Vector3D Testing)
{
/* float Line1Mag = Magnitude(VectorMinus(P1LineSegmentAB, P2LineSegmentAB));
float Line2Mag = Magnitude(VectorMinus(P1LineSegmentCD, P2LineSegmentCD));
P2LineSegmentAB = VectorMinus(P2LineSegmentAB, P1LineSegmentAB);
P1LineSegmentCD = VectorMinus(P1LineSegmentCD, P1LineSegmentAB);
P2LineSegmentCD = VectorMinus(P2LineSegmentCD, P1LineSegmentAB);
P1LineSegmentAB = VectorMinus(P1LineSegmentAB, P1LineSegmentAB);
Vector3D P1P2UnitDirection = GetUnitVector(P2LineSegmentAB, { 0,0,0 });
AngleBetweenTwoVectorsWithCommonUnitVectorAngleOfSecondArgument(P1LineSegmentAB, P2LineSegmentAB, P1P2UnitDirection);*/
Vector3D ReturnVal;
Vector3D PointCD1AB;
Vector3D PointCD2AB;
Vector3D PointAB1CD;
Vector3D PointAB2CD;
NearestPointOnLineFromPoint(P1LineSegmentCD, P1LineSegmentAB, P2LineSegmentAB, PointCD1AB, false);
PrintVector3Dfor(VectorMinus(PointCD1AB, Testing), "PointCD1AB", true);
NearestPointOnLineFromPoint(P2LineSegmentCD, P1LineSegmentAB, P2LineSegmentAB, PointCD2AB, false);
PrintVector3Dfor(VectorMinus(PointCD2AB, Testing), "PointCD2AB", true);
NearestPointOnLineFromPoint(P1LineSegmentAB, P1LineSegmentCD, P2LineSegmentCD, PointAB1CD, false);
PrintVector3Dfor(VectorMinus(PointAB1CD, Testing), "PointAB1CD", true);
NearestPointOnLineFromPoint(P2LineSegmentAB, P1LineSegmentCD, P2LineSegmentCD, PointAB2CD, false);
PrintVector3Dfor(VectorMinus(PointAB2CD, Testing), "PointAB2CD", true);
float m1 = Magnitude(VectorMinus(PointCD1AB, P1LineSegmentCD));
float m2 = Magnitude(VectorMinus(PointCD2AB, P2LineSegmentCD));
float m3 = Magnitude(VectorMinus(PointAB1CD, P1LineSegmentAB));
float m4 = Magnitude(VectorMinus(PointAB1CD, P2LineSegmentAB));
float m5 = Magnitude(VectorMinus(PointCD1AB, PointAB1CD));
float m6 = Magnitude(VectorMinus(PointCD2AB, PointAB2CD));
float m7 = Magnitude(VectorMinus(PointCD1AB, PointAB2CD));
float m8 = Magnitude(VectorMinus(PointCD2AB, PointAB1CD));
Printfloatfor(m1, "Magntidue of PointCD1AB - P1LineSegmentCD");
Printfloatfor(m2, "Magntidue of PointCD2AB - P2LineSegmentCD");
Printfloatfor(m3, "Magntidue of PointAB1CD - P1LineSegmentAB");
Printfloatfor(m4, "Magntidue of PointAB2CD - P2LineSegmentAB");
Printfloatfor(m5, "Magntidue of PointCD1AB - PointAB1CD");
Printfloatfor(m6, "Magntidue of PointCD2AB - PointAB2CD");
Printfloatfor(m7, "Magntidue of PointCD1AB - PointAB2CD");
Printfloatfor(m8, "Magntidue of PointCD2AB - PointAB1CD");
//NearestPointBetweenTwoLineSegmentOfSameLength1(P1LineSegmentAB, P2LineSegmentAB, P1LineSegmentCD, P2LineSegmentCD);
//NearestPointBetweenTwoLineSegmentOfSameLength2(P1LineSegmentAB, P2LineSegmentAB, P1LineSegmentCD, P2LineSegmentCD);
//NearestPointBetweenTwoLineSegmentOfSameLength3(P1LineSegmentAB, P2LineSegmentAB, P1LineSegmentCD, P2LineSegmentCD);
}
void NearestPointOnLineFromPoint(Vector3D Point, Vector3D LineSegmentStart, Vector3D LineSegmentEnd, Vector3D& ReturnVector, bool ClampTheValue)
{
//Get Heading Direction of Capsule from Origin To End
Vector3D CapsuleHeading = VectorMinus(LineSegmentEnd, LineSegmentStart);
float MagnitudeOfLineSegment = Magnitude(CapsuleHeading);
CapsuleHeading = VectorDivide(CapsuleHeading, MagnitudeOfLineSegment);
// Project From Point to Origin
Vector3D Projection = VectorMinus(Point, LineSegmentStart);
float DotProd = DotProduct(Projection, CapsuleHeading);
if (ClampTheValue)
{
DotProd = Clamp(DotProd, 0.0f, MagnitudeOfLineSegment);
}
ReturnVector = VectorAdd(LineSegmentStart, VectorMultiply(CapsuleHeading, DotProd));
}
I have Converted This Code from C# to C++ and it is not working as intended... I don't know if there is a problem with my code conversion or a problem within the code itself?
Vector3D ClampPointToLine(Vector3D pointToClamp, Vector3D LineStart, Vector3D LineEnd)
{
Vector3D clampedPoint = {0,0,0};
double minX, minY, minZ, maxX, maxY, maxZ;
if (LineStart.x <= LineEnd.x)
{
minX = LineStart.x;
maxX = LineEnd.x;
}
else
{
minX = LineEnd.x;
maxX = LineStart.x;
}
if (LineStart.y <= LineEnd.y)
{
minY = LineStart.y;
maxY = LineEnd.y;
}
else
{
minY = LineEnd.y;
maxY = LineStart.y;
}
if (LineStart.z <= LineEnd.z)
{
minZ = LineStart.z;
maxZ = LineEnd.z;
}
else
{
minZ = LineEnd.z;
maxZ = LineStart.z;
}
clampedPoint.x = (pointToClamp.x < minX) ? minX : (pointToClamp.x > maxX) ? maxX : pointToClamp.x;
clampedPoint.y = (pointToClamp.y < minY) ? minY : (pointToClamp.y > maxY) ? maxY : pointToClamp.y;
clampedPoint.z = (pointToClamp.z < minZ) ? minZ : (pointToClamp.z > maxZ) ? maxZ : pointToClamp.z;
return clampedPoint;
}
void distBetweenLines(Vector3D p1, Vector3D p2, Vector3D p3, Vector3D p4, Vector3D& ClosestPointOnLineP1P2, Vector3D& ClosestPointOnLineP3P4)
{
Vector3D d1;
Vector3D d2;
d1 = VectorMinus(p2, p1);
d2 = VectorMinus(p4, p3);
double eq1nCoeff = (d1.x * d2.x) + (d1.y * d2.y) + (d1.z * d2.z);
double eq1mCoeff = (-(powf(d1.x, 2)) - (powf(d1.y, 2)) - (powf(d1.z, 2)));
double eq1Const = ((d1.x * p3.x) - (d1.x * p1.x) + (d1.y * p3.y) - (d1.y * p1.y) + (d1.z * p3.z) - (d1.z * p1.z));
double eq2nCoeff = ((powf(d2.x, 2)) + (powf(d2.y, 2)) + (powf(d2.z, 2)));
double eq2mCoeff = -(d1.x * d2.x) - (d1.y * d2.y) - (d1.z * d2.z);
double eq2Const = ((d2.x * p3.x) - (d2.x * p1.x) + (d2.y * p3.y) - (d2.y * p2.y) + (d2.z * p3.z) - (d2.z * p1.z));
double M[2][3] = { { eq1nCoeff, eq1mCoeff, -eq1Const }, { eq2nCoeff, eq2mCoeff, -eq2Const } };
int rowCount = 2;
// pivoting
for (int col = 0; col + 1 < rowCount; col++) if (M[col, col] == 0)
// check for zero coefficients
{
// find non-zero coefficient
int swapRow = col + 1;
for (; swapRow < rowCount; swapRow++) if (M[swapRow, col] != 0) break;
if (M[swapRow, col] != 0) // found a non-zero coefficient?
{
// yes, then swap it with the above
double tmp[2];
for (int i = 0; i < rowCount + 1; i++)
{
tmp[i] = M[swapRow][i];
M[swapRow][i] = M[col][i];
M[col][i] = tmp[i];
}
}
else
{
std::cout << "\n the matrix has no unique solution";
return; // no, then the matrix has no unique solution
}
}
// elimination
for (int sourceRow = 0; sourceRow + 1 < rowCount; sourceRow++)
{
for (int destRow = sourceRow + 1; destRow < rowCount; destRow++)
{
double df = M[sourceRow][sourceRow];
double sf = M[destRow][sourceRow];
for (int i = 0; i < rowCount + 1; i++)
M[destRow][i] = M[destRow][i] * df - M[sourceRow][i] * sf;
}
}
// back-insertion
for (int row = rowCount - 1; row >= 0; row--)
{
double f = M[row][row];
if (f == 0) return;
for (int i = 0; i < rowCount + 1; i++) M[row][i] /= f;
for (int destRow = 0; destRow < row; destRow++)
{
M[destRow][rowCount] -= M[destRow][row] * M[row][rowCount]; M[destRow][row] = 0;
}
}
double n = M[0][2];
double m = M[1][2];
Vector3D i1 = { p1.x + (m * d1.x), p1.y + (m * d1.y), p1.z + (m * d1.z) };
Vector3D i2 = { p3.x + (n * d2.x), p3.y + (n * d2.y), p3.z + (n * d2.z) };
Vector3D i1Clamped = ClampPointToLine(i1, p1, p2);
Vector3D i2Clamped = ClampPointToLine(i2, p3, p4);
ClosestPointOnLineP1P2 = i1Clamped;
ClosestPointOnLineP3P4 = i2Clamped;
return;
}
Your problem is to find the shortest connection P1P2 between two line segments AB and CD. Let us define l1 as the line which goes through the points A and B and l2 as the line which goes through C and D.
You can split this problem up into several subtasks:
finding the shortest connection between the lines l1 and l2.
finding the shortest connection from either of the points A, B to segment CD (likewise for C,D to segment AB).
Let's start with the first subtask. THe line l1, going through A and B, can be parametrised by a single scalar, say sc,
l1(sc) = u*sc + A
with the direction vector u=(B-A).
As a consequence, we also have l1(0) = A and l(1) = B. Now, we want to find the minimal distance between this line and another line going through C and D, i.e.
l2(c) = v*tc + C
with v = D-C. In analogy to the other line, we have have l2(0) = C and l(1) = D. Now, we define
f(sc, tc) = 1/2*|l1(sc)-l2(tc)|^2
which is nothing but half the distance between the two lines squared. If we now want to minimise this function, we need to satisfy
df/dsc = 0 and df/dtc = 0
You'll find that
df/dsc = [u*sc - v*tc + (A-C)]*u and df/dtc = [u*sc - v*tc + (A-C)]*(-v)
Introducing w=A-C and arranging in vectors and matrices yields:
[ u*u -v*u] * [sc] = -[ w*u]
[-u*v v*v] [tc] [-w*v]
m * result = -rhs
The solution of the linear system is result = -m^(⁻1)* rhs, where m^(⁻1) is the inverse of m. If a and c are less than 0 or greater than 1, the closest point of the lines is outside the segments AB and CD. You might return these values as well.
The second subtask is closely related to this problem, but we minimise
f(sc) = 1/2*|l1(sc)-P|^2 and g(tc) = 1/2*|l2(tc)-P|^2
which directly yields
sc = -(A-P)*u/(u*u) and rc = -(C-P)*v/(v*v)
If sc < 0 we set sc = 0 or if sc > 1 we set sc = 1 (and likewise for tc) in order to get points on the segments.
Here is the implementation, which I took from here and modified it.
First, we define some helpers, i.e. vectors and some basic mathematical relations.
template <int dim>
struct Vector
{
std::array<double, dim> components;
};
using Vector2D = Vector<2>;
using Vector3D = Vector<3>;
// subtract
template <int dim>
Vector<dim> operator-(const Vector<dim> &u, const Vector<dim> &v) {
Vector<dim> result(u);
for (int i = 0; i < dim; ++i)
result.components[i] -= v.components[i];
return result;
}
// add
template <int dim>
Vector<dim> operator+(const Vector<dim> &u, const Vector<dim> &v) {
Vector<dim> result(u);
for (int i = 0; i < dim; ++i)
result.components[i] += v.components[i];
return result;
}
// negate
template <int dim>
Vector<dim> operator-(const Vector<dim> &u) {
Vector<dim> result;
for (int i = 0; i < dim; ++i)
result.components[i] = -u.components[i];
return result;
}
// scalar product
template <int dim>
double operator*(const Vector<dim> &u, const Vector<dim> &v) {
double result = 0;
for (int i = 0; i < dim; ++i)
result += u.components[i] * v.components[i];
return result;
}
// scale
template <int dim>
Vector<dim> operator*(const Vector<dim> &u, const double s) {
Vector<dim> result(u);
for (int i = 0; i < dim; ++i)
result.components[i] *= s;
return result;
}
// scale
template <int dim>
Vector<dim> operator*(const double s, const Vector<dim> &u) {
return u*s;
}
// ostream
template <int dim>
std::ostream& operator<< (std::ostream& out, const Vector<dim> &u) {
out << "(";
for (auto c : u.components)
out << std::setw(15) << c ;
out << ")";
return out;
}
This function does the actual work:
std::pair<Vector3D, Vector3D>
shortest_connection_segment_to_segment(const Vector3D A, const Vector3D B,
const Vector3D C, const Vector3D D)
{
Vector3D u = B - A;
Vector3D v = D - C;
Vector3D w = A - C;
double a = u*u; // always >= 0
double b = u*v;
double c = v*v; // always >= 0
double d = u*w;
double e = v*w;
double sc, sN, sD = a*c - b*b; // sc = sN / sD, sD >= 0
double tc, tN, tD = a*c - b*b; // tc = tN / tD, tD >= 0
double tol = 1e-15;
// compute the line parameters of the two closest points
if (sD < tol) { // the lines are almost parallel
sN = 0.0; // force using point A on segment AB
sD = 1.0; // to prevent possible division by 0.0 later
tN = e;
tD = c;
}
else { // get the closest points on the infinite lines
sN = (b*e - c*d);
tN = (a*e - b*d);
if (sN < 0.0) { // sc < 0 => the s=0 edge is visible
sN = 0.0; // compute shortest connection of A to segment CD
tN = e;
tD = c;
}
else if (sN > sD) { // sc > 1 => the s=1 edge is visible
sN = sD; // compute shortest connection of B to segment CD
tN = e + b;
tD = c;
}
}
if (tN < 0.0) { // tc < 0 => the t=0 edge is visible
tN = 0.0;
// recompute sc for this edge
if (-d < 0.0) // compute shortest connection of C to segment AB
sN = 0.0;
else if (-d > a)
sN = sD;
else {
sN = -d;
sD = a;
}
}
else if (tN > tD) { // tc > 1 => the t=1 edge is visible
tN = tD;
// recompute sc for this edge
if ((-d + b) < 0.0) // compute shortest connection of D to segment AB
sN = 0;
else if ((-d + b) > a)
sN = sD;
else {
sN = (-d + b);
sD = a;
}
}
// finally do the division to get sc and tc
sc = (fabs(sN) < tol ? 0.0 : sN / sD);
tc = (fabs(tN) < tol ? 0.0 : tN / tD);
Vector3D P1 = A + (sc * u);
Vector3D P2 = C + (tc * v);
return {P1, P2}; // return the closest distance
}
Usage:
int main() {
Vector3D A = {-7.54, 6.55, 0 };
Vector3D B = {4.54, -3.87, 6.0 };
Vector3D C = {0.0, 8.0, 3.53 };
Vector3D D = {0.03, -7.24, -1.34 };
auto [P1, P2] = shortest_connection_segment_to_segment (A, B, C, D);
std::cout << "P1 = " << P1 << std::endl;
std::cout << "P2 = " << P2 << std::endl;
return 0;
}
This prints
P1 = ( -1.24635 1.1212 3.12599)
P2 = ( 0.0125125 1.64365 1.49881)
live demo.
Note that this code still requires more testing.
Below Is a "Compact" version of the code from #StefanKssmr which is Here, This "Compact" version can easily be ported to OpenCL
Many thanks to #StefanKssmr for posting the Correct Answer,
void NearestPointBetweenTwoLineSegment(Vector3D AB1, Vector3D AB2, Vector3D CD1, Vector3D CD2, Vector3D& resultSegmentPoint1, Vector3D& resultSegmentPoint2)
{
Vector3D u = VectorMinus(AB2, AB1);
Vector3D v = VectorMinus(CD2, CD1);
Vector3D w = VectorMinus(AB1, CD1);
double a = DotProduct(u, u); // always >= 0
double b = DotProduct(u, v);
double c = DotProduct(v, v); // always >= 0
double d = DotProduct(u, w);
double e = DotProduct(v, w);
double sN, sD = (a * c) - (b * b); // sc = sN / sD, default sD = D >= 0
double tN, tD = (a * c) - (b * b); // tc = tN / tD, default tD = D >= 0
float Temp1;
float Temp2;
float Temp3;// Unfortuantely i have no choice but to use this...
//Part 1
Temp1 = (sD < 1e-6f) ? 1.0f : 0.0f;
sN = (1.0f - Temp1) * (b * e - c * d);
sD = ((1.0f - Temp1) * sD) + Temp1;
tN = (Temp1 * e) + ((1.0f - Temp1) * ((a * e) - (b * d)));
tD = (Temp1 * c) + ((1.0f - Temp1) * tD);
Temp2 = (sN < 0.0f) ? 1.0f : 0.0f;
Temp2 = Temp2 * (1.0f - Temp1);
sN = ((1.0f - Temp2) * sN);
tN = ((1.0f - Temp2) * tN) + (Temp2 * e);
tD = ((1.0f - Temp2) * tD) + (Temp2 * c);
Temp2 = ((sN > sD) ? 1.0f : 0.0f) * (1.0f - Temp2);
Temp2 = Temp2 * (1.0f - Temp1);
sN = ((1.0f - Temp2) * sN) + (Temp2 * sD);
tN = ((1.0f - Temp2) * tN) + (Temp2 * (e + b));
tD = ((1.0f - Temp2) * tD) + (Temp2 * c);
//Part 2.1
Temp1 = (tN < 0.0f) ? 1.0f : 0.0f;
tN = tN * (1.0f - Temp1);
Temp2 = (((-d) < 0.0) ? 1.0f : 0.0f) * Temp1;
sN = (1.0f - Temp2) * sN;//sN = (Temp2 * 0) + ((1.0f - Temp2) * sN);
Temp3 = ((((-d) > a) ? 1.0f : 0.0f) * (1.0f - Temp2)) * (Temp1);
sN = (Temp3 * sD) + ((1.0f - Temp3) * (sN));
Temp2 = (1.0f - Temp3) * (1.0f - Temp2) * (Temp1);
sN = (Temp2 * (-d)) + ((1.0f - Temp2) * (sN));
sD = (Temp2 * a) + ((1.0f - Temp2) * (sD));
//Part 2.2
Temp1 = ((tN > tD) ? 1.0f : 0.0f) * (1.0f - Temp1);
tN = ((1.0f - Temp1) * tN) + (Temp1 * tD);
Temp2 = (((-d + b) < 0.0) ? 1.0f : 0.0f) * Temp1;
sN = (1.0f - Temp2) * sN;//sN = (Temp2 * 0) + ((1.0f - Temp2) * sN);
Temp3 = ((((-d + b) > a) ? 1.0f : 0.0f) * (1.0f - Temp2)) * (Temp1);
sN = (Temp3 * sD) + ((1.0f - Temp3) * (sN));
Temp2 = (1.0f - Temp3) * (1.0f - Temp2) * (Temp1);
sN = (Temp2 * (-d)) + ((1.0f - Temp2) * (sN));
sD = (Temp2 * a) + ((1.0f - Temp2) * (sD));
resultSegmentPoint1 = VectorAdd(AB1, VectorMultiply(u, (fabs(sN) < 1e-6f ? 0.0 : sN / sD)));
resultSegmentPoint2 = VectorAdd(CD1, VectorMultiply(v, (fabs(tN) < 1e-6f ? 0.0 : tN / tD)));
}
I am programming a physics simulation with few particles (typically 3, no more than 5).
In a condensed version my code structure like this:
#include<iostream>
class Particle{
double x; // coordinate
double m; // mass
};
void performStep(Particle &p, double &F_external){
p.x += -0.2*p.x + F_external/p.m; // boiled down, in reality complex calculation, not important here
}
int main(){
dt = 0.001; // time step, not important
Particle p1;
p1.x = 5; // some random number for initialization, in reality more complex but not important here
p.m = 1;
Particle p2;
p2.x = -1; // some random numbersfor initialization, in reality more complex but not important here
p.m = 2;
Particle p3;
p3.x = 0; // some random number for initialization, in reality more complex but not important here
p.m = 3;
double F_external = 0; // external forces
for(unsigned long long int i=0; i < 10000000000; ++i){ // many steps, typically 10e9
F_external = sin(i*dt);
performStep(p1, F_external);
performStep(p2, F_external);
performStep(p3, F_external);
}
std::cout << "p1.x: " << p1.x << std::endl;
std::cout << "p2.x: " << p2.x << std::endl;
std::cout << "p3.x: " << p3.x << std::endl;
}
I have determined with clock() that the performStep(p, F_external) call is the bottleneck in my code).
When I tried to do inline calculation, i.e. replace performStep(p1, F_external) by p1.x += -0.2*p1.x + F_external/p1.m; the calculation suddenly was roughly a factor of 2 faster. Note that performStep() in reality is about ~60 basic arithmetic calculations over ~20 lines, so the code becomes really bloated if I just inline it for every particle.
Why is that the case? I am compiling with MinGW64/g++ and the -O2 flag. I thought the compiler would optimize such things?
Edit:
Here is the function that is called. Note that in reality, I calculate all three coordinates x,y,z with a couple of different external forces. Variables which are not passed via the function are a member of SimulationRun. The algorithm is a fourth-order leapfrog algorithm.
void SimulationRun::performLeapfrog_z(const unsigned long long int& i, const double& x, const double& y, double& z, const double& vx, const double& vy, double& vz, const double& qC2U0,
const double& U0, const double& m, const double& C4, const double& B2, const double& f_minus, const double& f_z, const double& f_plus, const bool& bool_calculate_xy,
const double& Find, const double& Fheating) {
// probing for C4 == 0 and B2 == 0 saves some computation time
if (C4 == 0) {
Fz_C4_Be = 0;
}
if (B2 == 0 || !bool_calculate_xy) {
Fz_B2_Be = 0;
}
z1 = z + c1 * vz * dt;
if (C4 != 0 && !bool_calculate_xy) {
Fz_C4_Be = (-4) * q * C4 * U0 * z1 * z1 * z1;
}
else if (C4 != 0 && bool_calculate_xy) {
Fz_C4_Be = q * C4 * U0 * (-4 * z1 * z1 * z1 + 6 * z1 * (x * x + y * y));
}
if (B2 != 0 && bool_calculate_xy) {
Fz_B2_Be = q * B2 * (-vx * z1 * y + vy * z1 * x);
}
acc_z1 = (qC2U0 * (-2) * z1 + Find + Fz_C4_Be + Fz_B2_Be + Fheating) / m;
vz1 = vz + d1 * acc_z1 * dt;
z2 = z1 + c2 * vz1 * dt;
if (C4 != 0 && !bool_calculate_xy) {
Fz_C4_Be = (-4) * q * C4 * U0 * z2 * z2 * z2;
}
else if (C4 != 0 && bool_calculate_xy) {
Fz_C4_Be = q * C4 * U0 * (-4 * z2 * z2 * z2 + 6 * z2 * (x * x + y * y));
}
if (B2 != 0 && bool_calculate_xy) {
Fz_B2_Be = q * B2 * (-vx * z2 * y + vy * z2 * x);
}
acc_z2 = (qC2U0 * (-2) * z2 + +Find + Fz_C4_Be + Fz_B2_Be + Fheating) / m;
vz2 = vz1 + d2 * acc_z2 * dt;
z3 = z2 + c3 * vz2 * dt;
if (C4 != 0 && !bool_calculate_xy) {
Fz_C4_Be = (-4) * q * C4 * U0 * z3 * z3 * z3;
}
else if (C4 != 0 && bool_calculate_xy) {
Fz_C4_Be = q * C4 * U0 * (-4 * z3 * z3 * z3 + 6 * z3 * (x * x + y * y));
}
if (B2 != 0 && bool_calculate_xy) {
Fz_B2_Be = q * B2 * (-vx * z3 * y + vy * z3 * x);
}
acc_z3 = (qC2U0 * (-2) * z3 + Find + Fz_C4_Be + Fz_B2_Be + Fheating) / m;
vz3 = vz2 + d3 * acc_z3 * dt;
z = z3 + c4 * vz3 * dt;
vz = vz3;
}
Optimization is hard, even for compilers. Here are some optimization tips:
Since your performStep is hotspot, put it into a header file(in case that you split declaration and definition into header/source), then add inline keyword, like:
// at file xxx.h
inline void performStep(Particle &p, double F_external){
p.x += -0.2*p.x + F_external/p.m; // boiled down, in reality complex calculation, not important here
}
Upgrade your compiler, maybe to the latest.
use https://godbolt.org/ to check the assembly code. In this case, unnecessary dereference is the headache of performance.
I've been poking my nose into working with arrays in c++ and I've been writing a 1D Euler solver code that I wrote in matlab and converting it to c++ as a practice exercise.
This issue is that this for loop is supposed to run until the counter i reaches N_cells-1 but no matter how high I set the number, it always gets to 57, then restarts from 2 and continues doing this until I click on the output screen. I also ran the code with an N_cells number less than 57 and I get an error code which I've included below.
I'm pretty new to arrays and header files in c++ so I'm sure it's something simple, but I just can't find it. I know it's related to the fqL array but I don't know what.
Error when number <57 is used:
#include "stdafx.h"
#include "Flux.h"
#include <iostream>
#include <chrono>
using namespace std;
void Flux(double * q, double y, double R, int N_cells,double * Flux)
{
double qL[3];
double qR[3];
for (int i = 0; i < N_cells - 1; i++) {
//Initialize left and right sides
//-------------------
qL[0] = q[0, i];
qL[1] = q[1, i];
qL[2] = q[2, i];
qR[0] = q[0, i + 1];
qR[1] = q[1, i + 1];
qR[2] = q[2, i + 1];
//-------------------
//Calculate Left side Parameters
//-------------------
double PL;
//double fqL[3];
double cL2;
double HL;
double uL;
PL = (y - 1)*(qL[2] - 0.5 / qL[0] * (qL[1] * qL[1]));
double fqL[3] = { qL[1],
(3 - y)*0.5*(qL[1] * qL[1]) / qL[0] + (y - 1)*qL[2],
y*qL[1] * qL[2] / qL[0] - (y - 1)*0.5*(qL[1] * qL[1] * qL[1]) / (qL[0] * qL[0]) };
cL2 = y * (y - 1)*(qL[2] / qL[0] - 0.5*(qL[1] / qL[0])*(qL[1] / qL[0]));
HL = 0.5*(qL[1] / qL[0])*(qL[1] / qL[0]) + cL2 / (y - 1);
uL = qL[1] / qL[0];
//Calculate Right side Parameters
//-------------------
double PR;
//double fqR[3];
double cR2;
double HR;
double uR;
PR = (y - 1)*(qR[2] - 0.5 / qR[0] * (qR[1] * qR[1]));
double fqR[3] = { qR[1],
(3 - y)*0.5*(qR[1] * qR[1]) / qR[0] + (y - 1)*qR[2],
y*qR[1] * qR[2] / qR[0] - (y - 1)*0.5*(qR[1] * qR[1] * qR[1]) / (qR[0] * qR[0]) };
cR2 = y * (y - 1)*(qR[2] / qR[0] - 0.5*(qR[1] / qR[0])*(qR[1] / qR[0]));
HR = 0.5*(qR[1] / qR[0])*(qR[1] / qR[0]) + cR2 / (y - 1);
uR = qR[1] / qR[0];
//-------------------
//Calculate Roe's Variables
//-------------------------------- -
double u;
double H;
double c;
double rho;
u = (sqrt(qL[1])*qL[2] / qL[1] + sqrt(qR[1])*qR[2] / qR[1]) / (sqrt(qL[1]) + sqrt(qR[1]));
H = (sqrt(qL[1])*HL + sqrt(qR[1])*HR) / (sqrt(qL[1]) + sqrt(qR[1]));
c = sqrt((y - 1)*(H - 0.5*u *u));
rho = sqrt(qL[1] * qR[1]);
//-------------------------------- -
//-------------------------------- -
double g[3] = { u - c, u, u + c };
double v[3][3] = { {1, u - c, H - u * c},
{1, u, 0.5*u*u},
{1, u + c, H + u * c } };
double a[3] = { 0.5 / (c*c)*((PR - PL) - c * rho*(uR - uL)),
(qR[0] - qL[0]) - 1 * (PR - PL) / (c*c),
0.5 / (c*c)*((PR - PL) + c * rho*(uR - uL)) };
double SUM[3];
SUM[0] = g[0] * a[0] * v[0][0] + g[1] * a[1] * v[1][0] + g[2] * a[2] * v[2][0];
SUM[1] = g[0] * a[0] * v[0][1] + g[1] * a[1] * v[1][1] + g[2] * a[2] * v[2][1];
SUM[2] = g[0] * a[0] * v[0][2] + g[1] * a[1] * v[1][2] + g[2] * a[2] * v[2][2];
double Flux[3];
Flux[0,i] = 0.5*(fqL[0] + fqR[0]) - 0.5*SUM[0];
Flux[1,i] = 0.5*(fqL[1] + fqR[1]) - 0.5*SUM[1];
Flux[2,i] = 0.5*(fqL[2] + fqR[2]) - 0.5*SUM[2];
std::cout << i << endl;
}
}
I am using this code for calculating sunrise and sunset times.
// Get the daylight status of the current time.
bool
SunLight::CalculateDaylightStatus()
{
// Calculate the current time of day.
time_t currentTime = time(NULL);
m_LocalTime = localtime(¤tTime);
// Initialize the sunrise and set times.
*m_Sunrise = *m_LocalTime;
*m_Sunset = *m_LocalTime;
// Flags to check whether sunrise or set available on the day or not.
m_IsSunrise = false;
m_IsSunset = false;
m_RiseAzimuth = 0.0;
m_SetAzimuth = 0.0;
for (unsigned int i = 0; i < 3; i++)
{
m_RightAscention[i] = 0.0;
m_Decension[i] = 0.0;
m_VHz[i] = 0.0;
}
for (unsigned int i = 0; i < 2; i++)
{
m_SunPositionInSky[i] = 0.0;
m_RiseTime[i] = 0;
m_SetTime[i] = 0;
}
// Calculate the sunrise and set times.
CalculateSunRiseSetTimes();
return (mktime(m_LocalTime) >= mktime(m_Sunrise) && mktime(m_LocalTime) < mktime(m_Sunset))
? true
: false;
}
//---------------------------------------------------------------------
bool
SunLight::CalculateSunRiseSetTimes()
{
double zone = timezone/3600 - m_LocalTime->tm_isdst;
// Julian day relative to Jan 1.5, 2000.
double jd = GetJulianDay() - 2451545;
if ((Sign(zone) == Sign(m_Config->Longitude())) && (zone != 0))
{
return false;
}
double tz = zone / 24;
// Centuries since 1900.0
double ct = jd / 36525 + 1;
// Local sidereal time.
double t0 = LocalSiderealTimeForTimeZone(jd, tz, m_Config->Longitude()/360);
// Get sun position at start of day.
jd += tz;
// Calculate the position of the sun.
CalculateSunPosition(jd, ct);
double ra0 = m_SunPositionInSky[0];
double dec0 = m_SunPositionInSky[1];
// Get sun position at end of day.
jd += 1;
// Calculate the position of the sun.
CalculateSunPosition(jd, ct);
double ra1 = m_SunPositionInSky[0];
double dec1 = m_SunPositionInSky[1];
// make continuous
if (ra1 < ra0)
ra1 += 2 * M_PI;
m_RightAscention[0] = ra0;
m_Decension[0] = dec0;
// check each hour of this day
for (int k = 0; k < 24; k++)
{
m_RightAscention[2] = ra0 + (k + 1) * (ra1 - ra0) / 24;
m_Decension[2] = dec0 + (k + 1) * (dec1 - dec0) / 24;
m_VHz[2] = TestHour(k, t0, m_Config->Latitude());
// advance to next hour
m_RightAscention[0] = m_RightAscention[2];
m_Decension[0] = m_Decension[2];
m_VHz[0] = m_VHz[2];
}
// Update the tm structure with time values.
m_Sunrise->tm_hour = m_RiseTime[0];
m_Sunrise->tm_min = m_RiseTime[1];
m_Sunset->tm_hour = m_SetTime[0];
m_Sunset->tm_min = m_SetTime[1];
// neither sunrise nor sunset
if ((!m_IsSunrise) && (!m_IsSunset))
{
// Sun down all day.
if (m_VHz[2] < 0)
m_IsSunset = true;
// Sun up all day.
else
m_IsSunrise = true;
}
return true;
}
//---------------------------------------------------------------------
int
SunLight::Sign(double value)
{
if (value > 0.0)
return 1;
else if (value < 0.0)
return -1;
else
return 0;
}
//---------------------------------------------------------------------
// Local Sidereal Time for zone.
double
SunLight::LocalSiderealTimeForTimeZone(double jd, double z, double lon)
{
double s = 24110.5 + 8640184.812999999 * jd / 36525 + 86636.6 * z + 86400 * lon;
s = s / 86400;
s = s - floor(s);
return s * 360 * cDegToRad;
}
//---------------------------------------------------------------------
// Determine Julian day from calendar date
// (Jean Meeus, "Astronomical Algorithms", Willmann-Bell, 1991).
double
SunLight::GetJulianDay()
{
int month = m_LocalTime->tm_mon + 1;
int day = m_LocalTime->tm_mday;
int year = 1900 + m_LocalTime->tm_year;
bool gregorian = (year < 1583) ? false : true;
if ((month == 1) || (month == 2))
{
year = year - 1;
month = month + 12;
}
double a = floor((double)year / 100);
double b = 0;
if (gregorian)
b = 2 - a + floor(a / 4);
else
b = 0.0;
double jd = floor(365.25 * (year + 4716))
+ floor(30.6001 * (month + 1))
+ day + b - 1524.5;
return jd;
}
//---------------------------------------------------------------------
// Sun's position using fundamental arguments
// (Van Flandern & Pulkkinen, 1979).
void
SunLight::CalculateSunPosition(double jd, double ct)
{
double g, lo, s, u, v, w;
lo = 0.779072 + 0.00273790931 * jd;
lo = lo - floor(lo);
lo = lo * 2 * M_PI;
g = 0.993126 + 0.0027377785 * jd;
g = g - floor(g);
g = g * 2 * M_PI;
v = 0.39785 * sin(lo);
v = v - 0.01 * sin(lo - g);
v = v + 0.00333 * sin(lo + g);
v = v - 0.00021 * ct * sin(lo);
u = 1 - 0.03349 * cos(g);
u = u - 0.00014 * cos(2 * lo);
u = u + 0.00008 * cos(lo);
w = -0.0001 - 0.04129 * sin(2 * lo);
w = w + 0.03211 * sin(g);
w = w + 0.00104 * sin(2 * lo - g);
w = w - 0.00035 * sin(2 * lo + g);
w = w - 0.00008 * ct * sin(g);
// compute sun's right ascension
s = w / sqrt(u - v * v);
m_SunPositionInSky[0] = lo + atan(s / sqrt(1 - s * s));
// ...and declination
s = v / sqrt(u);
m_SunPositionInSky[1] = atan(s / sqrt(1 - s * s));
}
//---------------------------------------------------------------------
// Test an hour for an event.
double
SunLight::TestHour(int k, double t0, double prmLatitude)
{
double ha[3];
double a, b, c, d, e, s, z;
double time;
double az, dz, hz, nz;
int hr, min;
ha[0] = t0 - m_RightAscention[0] + k * cK1;
ha[2] = t0 - m_RightAscention[2] + k * cK1 + cK1;
ha[1] = (ha[2] + ha[0]) / 2; // hour angle at half hour
m_Decension[1] = (m_Decension[2] + m_Decension[0]) / 2; // declination at half hour
s = sin(prmLatitude * cDegToRad);
c = cos(prmLatitude * cDegToRad);
z = cos(90.833 * cDegToRad); // refraction + sun semi-diameter at horizon
if (k <= 0)
m_VHz[0] = s * sin(m_Decension[0]) + c * cos(m_Decension[0]) * cos(ha[0]) - z;
m_VHz[2] = s * sin(m_Decension[2]) + c * cos(m_Decension[2]) * cos(ha[2]) - z;
if (Sign(m_VHz[0]) == Sign(m_VHz[2]))
return m_VHz[2]; // no event this hour
m_VHz[1] = s * sin(m_Decension[1]) + c * cos(m_Decension[1]) * cos(ha[1]) - z;
a = 2 * m_VHz[0] - 4 * m_VHz[1] + 2 * m_VHz[2];
b = -3 * m_VHz[0] + 4 * m_VHz[1] - m_VHz[2];
d = b * b - 4 * a * m_VHz[0];
if (d < 0)
return m_VHz[2]; // no event this hour
d = sqrt(d);
e = (-b + d) / (2 * a);
if ((e > 1) || (e < 0))
e = (-b - d) / (2 * a);
time = (double)k + e + (double)1 / (double)120; // time of an event
hr = (int)floor(time);
min = (int)floor((time - hr) * 60);
hz = ha[0] + e * (ha[2] - ha[0]); // azimuth of the sun at the event
nz = -cos(m_Decension[1]) * sin(hz);
dz = c * sin(m_Decension[1]) - s * cos(m_Decension[1]) * cos(hz);
az = atan2(nz, dz) / cDegToRad;
if (az < 0) az = az + 360;
if ((m_VHz[0] < 0) && (m_VHz[2] > 0))
{
m_RiseTime[0] = hr;
m_RiseTime[1] = min;
m_RiseAzimuth = az;
m_IsSunrise = true;
}
if ((m_VHz[0] > 0) && (m_VHz[2] < 0))
{
m_SetTime[0] = hr;
m_SetTime[1] = min;
m_SetAzimuth = az;
m_IsSunset = true;
}
return m_VHz[2];
}
//---------------------------------------------------------------------
I need to introduce altitude in the formula which gives more accurate result. Can someone give me a quick solution what I have to modify to add altitude in the formula?
That algorithm is nowhere near calculating the times of sunrise and sunset. What you need is Jean Meeus' book "Astronomical Algorithms". You will need to account for the observer's longitude and latitude, the difference between dynamical time and universal time, and the eccentricity of the Earth's orbit to obtain even a low accuracy result.
This seems to be called sunrise equation. The formulas in that Wiki article are unbelievably simple, and they do account for the geographic location.