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.
Related
I have to apply the 4th order Runge Kutta method (RK) to the coupled equation in handbook of marinecraft hydrodynamics and motion control, page 154, equation 7.33, to determine v and r for every iteration. the RK method works when I use the functions separately, i.e., when I use the functions in separate programs. I have converted the equations from matrix form to linear equations. q1, q2, q3 and q4 are coefficients of v and r
The RK method is given as a function and is called twice to find v and r values. I can obtain the correct range of values of r as when rud increases, the values of v and r increase as well. when rud = rudmax, v and r should remain constant. however, v keeps increasing and doesn't stop. both v and r should be constant when rud = rudmax.
#include <iostream>
#include <fstream>
#include <cmath>
#include <iomanip>
using namespace std;
double dvdt(double r, double v);
double drdt(double v, double r);
double RKv1(double to, double vo, double t, double h);
double RKr1(double to, double ro, double t, double h);
double xo = 0; //initial X position
double yo = 0; //initial Y position
double x1; //final X position
double y11; //final Y position
double ti = 0; //initial time = 0
double tf; //time entered
double dt = 0.01; //time step (iteration time)
double rud = 0; //rudder angle
double m = 3.27385 * pow(10, 11); //mass of submarine (Kg)
double Iz = 2.31448 * pow(10, 12); //moment of inertia of submarine (kg.m^2)
double xg = 10.3; //x position of COG
double yg = 0; // position of COG
double u = 10; // surge velocity
double v, vo = 0; //sway velocity
double r, ro = 0; // yaw rate
double psio = 0; //initial ship heading
double psi1; //final ship heading
double rudmax; // max rudder angle
double au; //surge acceleration
double av; // sway acceleration
double ar; //angular acceleration
double X; //surge force
double Y; //sway force
double N; //turning moment
double h = 1;
//double q1, q2, q3, q4;
//matrices
//maneuvering and hydrodynamic coefficents
double Xu1 = -0.010087715;//-1.0467 * pow(10,-3);
double Yv1 = -0.290828106;//-23.889 * pow(10, -3);
double Yr1 = -0.008070039;//-1.0510 * pow(10, -3);
double Nv1 = -0.002928156;// 1.1531 * pow(10, -3);
double Nr1 = -0.056559574; //-0.50717 * pow(10, -3);
double Xu = -0.007115651;// -2.8763 * pow(10, -3);
double Yv = -0.35098699; //-38.948 * pow(10, -3);
double Yr = 0.091748747;// 2.0031 * pow(10, -3);
double Nv = -0.123433077;//-14.287 * pow(10, -3);
double Nr = -0.056559574;//-4.2267 * pow(10, -3);
double Yrud = -0.587323034;//1.4308 * pow(10, -3);
double Nrud = 0.991135206;// -0.71540 * pow(10, -3);
double a1 = m - Yv1;
double b1 = m * xg - Yr1;
double a2 = m * xg - Yr1;
double b2 = Iz - Nr1;
double det = a1 * b2 - a2 * b1;
double q1 = (-Yv * (Iz - Nr1) / det);
double q2 = ((m - Xu1) * u - Yr) * (-m * xg + Yr1) / det;
double q3 = ((Xu1 - Yv1) * u - Nv) * (-m * xg + Yr1) / det;
double q4 = ((m * xg - Yr1) * u - Nr) * (m - Yr1) / det;
int main()
{
std::ofstream fout; //for .CSV file
fout.open("results2.csv", ios::out | ios::app); //for .CSV file
//cout << det<<"\n";
/*cout << "surge velocity = ";
cin >> */ u = 10;
/*cout << "rudmax = ";
cin >> */rudmax = 30;
/*cout << "final time = ";
cin >>*/ tf = 100;
fout << "rud" << ", " << "ti" << ", " << "r" << ", " << "v" << "\n"; //for .CSV file
while (ti <= tf)
{
v = RKv1(ti, vo, ti + 0.025, h);
r = RKr1(ti, ro, ti + 0.025, h);
fout << rud << ", " << ti << ", " << r << ", " << v << "\n"; //for .CSV file
vo = v;
ro = r;
ti = ti + dt;
if (rudmax > 0)
{
if (rud < rudmax)
{
rud = rud + 0.005;
}
}
else if (rudmax < 0)
{
if (rud > rudmax)
{
rud = rud - 0.005;
}
}
}
}
/*---------------------------------------------------FUNCTIONS---------------------------------------------------*/
//functions for velocity and distance
double dvdt(double r, double v)
{
//return(-Yrud * rud - ((-Yv * (Iz - Nr1) / det) * v + ((m - Xu1) * u - Yr) * ((-m * xg - Yr1) / det )* r));
return(-Yrud * rud - q1 * v + q2 * r);
}
double drdt(double v, double r)
{
//return(-Nrud * rud - (((Xu1 - Yv1) * u - Nv) * ((-m * xg + Yr) / det) * v + ((m * xg - Yr1) * u - Nr) * ((m - Yr1) / det) * r));
return(-Nrud * rud - q3 * v + q4 * r);
}
//RK methods
//correction of rk method
double RKv1(double to, double vo, double t, double h)
{
int n = /*(int)((t - to) / h)*/ 1;
double k1, k2, k3, k4;
double v = vo;
for (int i = 1; i <= n; i++)
{
k1 = h * dvdt(ro, v);
k2 = h * dvdt(ro, v + 0.5 * k1);
k3 = h * dvdt(ro, v + 0.5 * k2);
k4 = h * dvdt(ro, v + k3);
v = v + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
to = to + h;
}
return v;
}
double RKr1(double to, double ro, double t, double h)
{
int n = /*(int)((t - to) / h)*/ 1;
double k1, k2, k3, k4;
double r = ro;
for (int i = 1; i <= n; i++)
{
k1 = h * drdt(vo, r);
k2 = h * drdt(vo, r + 0.5 * k1);
k3 = h * drdt(vo, r + 0.5 * k2);
k4 = h * drdt(vo, r + k3);
r = r + (1.0 / 6.0) * (k1 + 2 * k2 + 2 * k3 + k4);
to = to + h;
}
return r;
}
the results are stored in the results2.csv file.
snippet of the result. r (-28.59) remains constant whereas v (-84000) keep increasing
Any help/suggestions are appreciated. thanks!
Can I operate the elements of an array in a function(in the parameter)?
float f(i, u, v)
{
if (i == 1) {
return (u - v); //example of the returned value
}
if (i == 2) {
return (u + v);
}
}
int main()
{
int i;
float x[3],y1,y2,h;
x[1]=1;//value of the first element of x[m]
x[2]=1;
h=0.01;
for (i = 1; i <= 2; i++) {
y1=h * f(i, x[1], x[2]);
y2=h * f(i, x[1] + y1/2, x[2]+y1/2);
y3=h* f(i,x[1] + y2/2, x[2]+y2/2);
y4=h * f(i,x[1] + y3, x[2]+y3);
x[1]=x[1] + (y1+ 2 * y2 + 2 * y3+2 * y4)/ 6;
x[2]=x[2] + (y1+ 2 * y2 + 2 * y3+2 * y4)/ 6;
cout<<x[1]<<endl;
}
}
with:
x[1] and x[2] are the elements of the array x[m]
How can I operate elements of different arrays in parameter?
I would recommend you to try to compile your code, the compiler will give you some important hints as of what is wrong. Here is the code compiled online.
To make it compile i changed it to this:
#include <iostream>
using namespace std;
float f(int i, float u, float v) {
if (i == 1) {
return (u - v); // example of the returned value
}
// if (i == 2) { // This if-statement is not needed
return (u + v);
// }
}
int main() {
int i;
float x[3] = {0, 1, 1}; // x[0] is unused...?
float y1 = 0;
float y2 = 0;
float y3 = 0;
float y4 = 0;
const float h = 0.01;
for (int i = 1; i <= 2; i++) {
y1 = h * f(i, x[1], x[2]);
y2 = h * f(i, x[1] + y1 / 2, x[2] + y1 / 2);
y3 = h * f(i, x[1] + y2 / 2, x[2] + y2 / 2);
y4 = h * f(i, x[1] + y3, x[2] + y3);
x[1] = x[1] + (y1 + 2 * y2 + 2 * y3 + 2 * y4) / 6;
x[2] = x[2] + (y1 + 2 * y2 + 2 * y3 + 2 * y4) / 6;
cout << x[1] << endl;
}
}
Note the changes
You need to specify the types for the variables in the function f(...)
You need to define all variables before using them (a good rule is to specify everything right before you use it, and add const if not changed.
Remember to zero initialize variables that you are going to add to (y1, y2... etc)
Also I would recommend you to use x1 instead of x1, since you are mixing styles between x and y, and you are not using the zeroeth element. Like this
int main() {
int i;
float x1 = 1;
float x2 = 2;
float y1 = 0;
float y2 = 0;
float y3 = 0;
float y4 = 0;
const float h = 0.01;
for (int i = 1; i <= 2; i++) {
y1 = h * f(i, x1, x2);
y2 = h * f(i, x1 + y1 / 2, x2 + y1 / 2);
y3 = h * f(i, x1 + y2 / 2, x2 + y2 / 2);
y4 = h * f(i, x1 + y3, x2 + y3);
x1 = x1 + (y1 + 2 * y2 + 2 * y3 + 2 * y4) / 6;
x2 = x2 + (y1 + 2 * y2 + 2 * y3 + 2 * y4) / 6;
cout << x1 << endl;
}
}
Let's say A1&A2 are Two 3D points that make a line Segment.
T1,T2,T3 are Three 3D points that make a Triangle Polygon in 3D space.
Let P1 be a point on the Line segment, Let P2 be a point on the Triangle Polygon
P1 And P2 Are Closest To each other
Now, how can I Calculate P1 and P2 which method shall I use?
The question is now Solved Here Is the Answer
Right now I know how to find the closest point on a line segment from a point.
and Closest two Points between Two Line Segment.
I use this Below Function to find the closest line segment between two line segment
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
}
I guess you could spend a lot of time writing and debugging quite a lot of code if you do this 'by hand'. A better approach would be to formulate it as an instance of a general problem and then look for libraries that can solve this problem.
In this case the problem is 'constrained linear least squares' which is quite common.
The first thing to do is to introduce parameters, for example:
A point P on the line is given by
P = P1 + l*(P2-P1) where 0<=l<=1
A point T in the triangle is given by
T = T1 + s*(T2-T1) + t*(T3-T1)
where 0<=s<=1 and 0<=t<=1 and s+t<=1
The objective function is the distance squared between P and T, i.e.
d2 = ||P(l) - T(s,t)||^2
A little algebra casts this into the standard least squares form:
d2 = ||A*x - b ||^2 where
x = (l,s,t)'
A = (P2-P1 T1-T2 T1-T3)
b = T1-P1
and the constraints, as above, are
0<=l and l<=1
0<=s and s<=1
0<=t and t<=1
s+t <= 1
This function Below Gets the Closest 2 3D points(line segment) between a Triangle(defined by 3 points) and a Line Segment(Defined by 2 points). These math functions are based on #StefanKssmr 's Answer And #Spektre 's line closest(line l0,triangle t0). Here is the Entire Code Online Compiler with Example.
Here is the Visual Representation for the examples
void ClosestPointBetweenTriangleAndLineSegment(Vector3D LineStart, Vector3D LineEnd, Vector3D Triangle0, Vector3D Triangle1, Vector3D Triangle2, Vector3D& ClosestPointOnLine, Vector3D& ClosestPointOnTriangle)
{
ClosestPointOnLine = LineStart;//For FirstPart Calculation Only
ClosestPointOnTriangle;
Vector3D Return1 = LineEnd;//For FirstPart Calculation Only
Vector3D Return2;
NearestPointBetweenPointAndPlane(ClosestPointOnLine, Triangle0, Triangle1, Triangle2, ClosestPointOnTriangle);
NearestPointBetweenPointAndPlane(Return1, Triangle0, Triangle1, Triangle2, Return2);
if (Magnitude(VectorMinus(Return1, Return2)) < Magnitude(VectorMinus(ClosestPointOnLine, ClosestPointOnTriangle)))
{
ClosestPointOnLine = Return1;
ClosestPointOnTriangle = Return2;
}
if (FastNoCheckIsPointWithinTraingle(ClosestPointOnTriangle, Triangle0, Triangle1, Triangle2))
{
return;
}
else
{
NearestPointBetweenTwoLineSegment(LineStart, LineEnd, Triangle0, Triangle1, ClosestPointOnLine, ClosestPointOnTriangle);// Only For First
NearestPointBetweenTwoLineSegment(LineStart, LineEnd, Triangle0, Triangle2, Return1, Return2);
if (Magnitude(VectorMinus(Return1, Return2)) < Magnitude(VectorMinus(ClosestPointOnLine, ClosestPointOnTriangle)))
{
ClosestPointOnLine = Return1;
ClosestPointOnTriangle = Return2;
}
NearestPointBetweenTwoLineSegment(LineStart, LineEnd, Triangle1, Triangle2, Return1, Return2);
if (Magnitude(VectorMinus(Return1, Return2)) < Magnitude(VectorMinus(ClosestPointOnLine, ClosestPointOnTriangle)))
{
ClosestPointOnLine = Return1;
ClosestPointOnTriangle = Return2;
}
}
}
Below are the Helper Function for the above function All are based on common math
#include <iostream>
#include <cmath>
struct Vector3D
{
float x = 0;
float y = 0;
float z = 0;
};
struct Vector4D
{
float x = 0;
float y = 0;
float z = 0;
float d = 0;
};
Vector3D VectorAdd(Vector3D V1, Vector3D V2)
{
return { V1.x + V2.x, V1.y + V2.y, V1.z + V2.z };
}
Vector3D VectorMinus(Vector3D V1, Vector3D V2)
{
return { V1.x - V2.x, V1.y - V2.y, V1.z - V2.z };
}
Vector3D VectorMultiply(Vector3D V1, float Multiplier)
{
return { (V1.x * Multiplier), (V1.y * Multiplier), (V1.z * Multiplier) };
}
Vector3D VectorMultiplyVector(Vector3D V1, Vector3D V2)
{
return { (V1.x * V2.x), (V1.y * V2.y), (V1.z * V2.z) };
}
Vector3D VectorDivide(Vector3D V1, float Divider)
{
return{ (V1.x / Divider), (V1.y / Divider), (V1.z / Divider) };
}
float DotProduct(Vector3D V1, Vector3D V2)
{
return (float)(V1.x * V2.x) + (V1.y * V2.y) + (V1.z * V2.z);
}
Vector3D CrossProduct(Vector3D V1, Vector3D V2)
{
return { (V1.y * V2.z) - (V1.z * V2.y),
(V1.z * V2.x) - (V1.x * V2.z),
(V1.x * V2.y) - (V1.y * V2.x) };
}
Vector3D VectorPower(Vector3D V1, float Power)
{
return { pow(V1.x, Power), pow(V1.y, Power), pow(V1.z, Power) };
}
float VectorTotal(Vector3D V1)
{
return DotProduct(V1, { 1,1,1 });
}
float Magnitude(Vector3D Vector)
{
float CalculationFloat = (float)(sqrt(pow(Vector.x, 2.0) + pow(Vector.y, 2.0) + pow(Vector.z, 2.0)));
if (std::isnan(CalculationFloat) || std::isinf(CalculationFloat))
{
return 0;
}
return CalculationFloat;
}
float AreaOfTriangle3D(Vector3D VA, Vector3D VB, Vector3D VC)
{
return Magnitude(CrossProduct(VectorMinus(VB, VA), VectorMinus(VC, VA))) / 2;
}
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)));
}
bool FastNoCheckIsPointWithinTraingle(Vector3D Point, Vector3D Triangle0, Vector3D Triangle1, Vector3D Triangle2)
{
return fabsf((AreaOfTriangle3D(Point, Triangle1, Triangle2) + AreaOfTriangle3D(Triangle0, Point, Triangle2) + AreaOfTriangle3D(Triangle0, Triangle1, Point)) - AreaOfTriangle3D(Triangle0, Triangle1, Triangle2)) < 1.0f;
}
Vector4D equation_plane(float x1, float y1,
float z1, float x2,
float y2, float z2,
float x3, float y3, float z3)
{
float a1 = x2 - x1;
float b1 = y2 - y1;
float c1 = z2 - z1;
float a2 = x3 - x1;
float b2 = y3 - y1;
float c2 = z3 - z1;
float a = b1 * c2 - b2 * c1;
float b = a2 * c1 - a1 * c2;
float c = a1 * b2 - b1 * a2;
float d = (-a * x1 - b * y1 - c * z1);
//std::cout << std::fixed;
//std::cout << std::setprecision(2);
std::cout << "\nequation of plane is " << a << " x + " << b << " y + " << c << " z + ";
printf("%f", d);
std::cout << " = 0.";
return { a,b,c,d };
}
void NearestPointBetweenPointAndPlane(Vector3D Point, Vector3D Triangle0, Vector3D Triangle1, Vector3D Triangle2, Vector3D& resultSegmentPoint)
{
//Note: Plane equation is x + y + z + d = 0 format//
Vector4D Plane = equation_plane(Triangle0.x, Triangle0.y, Triangle0.z, Triangle1.x, Triangle1.y, Triangle1.z, Triangle2.x, Triangle2.y, Triangle2.z);
Vector3D PlanePartial;
PlanePartial.x = Plane.x;
PlanePartial.y = Plane.y;
PlanePartial.z = Plane.z;
resultSegmentPoint = VectorAdd(Point, VectorMultiply(PlanePartial, -((VectorTotal(VectorMultiplyVector(Point, PlanePartial)) + Plane.d) / VectorTotal(VectorPower(PlanePartial, 2)))));
}
Optional Debug Function
int Clamp(double Number, double Min, double Max)
{
if (Number > Max)
{
return Max;
}
if (Number < Min)
{
return Min;
}
return Number;
}
void PrintVector3Dfor(Vector3D Point, std::string Name, bool InvertXY)
{
int SpaceBar = 50;
SpaceBar = Clamp(SpaceBar - Name.length(), 0, 50);
std::cout << "\n" << Name << " =";
for (int i = 0; i < SpaceBar; ++i)
{
std::cout << " ";
}
std::cout << "(";
if (InvertXY)
{
printf("%f", Point.y);
printf(", %f", Point.x);
}
else
{
printf("%f", Point.x);
printf(", %f", Point.y);
}
printf(", %f )", Point.z);
}
void Printfloatfor(float val, std::string Name)
{
int SpaceBar = 50;
SpaceBar = Clamp(SpaceBar - Name.length(), 0, 50);
std::cout << "\n" << Name << " =";
for (int i = 0; i < SpaceBar; ++i)
{
std::cout << " ";
}
printf(" %f", val);
}
Could anyone help me about B-spline Curve error?
I want to draw B-spline Curve in c++, but even though all coordinates are positive, the segment's coordinate is negative.
This is B-spline Curve code.
void BSplineCurve(Dot &ControlPoint1, Dot &ControlPoint2,
Dot &ControlPoint3,Dot &ControlPoint4,
Dot &DrawCurve, double &t){
double t2 = t * t;
double t3 = t2 * t;
double mt3 = (1 - t) * (1 - t) * (1 - t);
double bi3 = mt3 / 6;
double bi2 = ((3 * t3) - (6 * t2) + 4) / 6;
double bi1 = ((-3 * t3) + (3 * t2) + (3 * t) + 1) / 6;
double bi = mt3 / 6;
DrawCurve.x = ControlPoint1.x * bi3;
DrawCurve.x += ControlPoint2.x * bi2;
DrawCurve.x += ControlPoint3.x * bi1;
DrawCurve.x += ControlPoint4.x * bi;
DrawCurve.y = ControlPoint1.y * bi3;
DrawCurve.y += ControlPoint2.y * bi2;
DrawCurve.y += ControlPoint3.y * bi1;
DrawCurve.y += ControlPoint4.y * bi;
}
This is Drawing Code.
double t = 3.f;
do{
if ((3 < t) && (t <= 4)) {
BSplineCurve(ControlPoint1, ControlPoint2, ControlPoint3, ControlPoint4, DrawCurve, t);
Draw1Dot(DrawCurve.x, DrawCurve.y, DrawCurve.R, DrawCurve.G, DrawCurve.B);
}
else if ((4 < t) && (t <= 5)) {
BSplineCurve(ControlPoint2, ControlPoint3, ControlPoint4, ControlPoint5, DrawCurve, t);
Draw1Dot(DrawCurve.x, DrawCurve.y, DrawCurve.R, DrawCurve.G, DrawCurve.B);
}
else if ((5 < t) && (t <= 6)) {
BSplineCurve(ControlPoint3, ControlPoint4, ControlPoint5, ControlPoint6, DrawCurve, t);
Draw1Dot(DrawCurve.x, DrawCurve.y, DrawCurve.R, DrawCurve.G, DrawCurve.B);
}
t += 0.001;
} while(t < 6.001);
This is Control Point's coordinate.
Poiont1 : 50, 50
Poiont2 : 50, 100
Poiont3 : 200, 100
Poiont4 : 200, 50
Poiont5 : 350, 50
Poiont6 : 350, 100
But this is 1st segment's coordinate.
Q3 : -1543, -349
Your drawing code looks wrong.
In function BSplineCurve the t parameter should take values in [0, 1] range. By changing t from 0 to 1 one will build a cubic B-spline between points ControlPoint2 and ControlPoint3.
You could try something like:
Dot points[6] = {ControlPoint1, ControlPoint2, ControlPoint3, ControlPoint4, ControlPoint5, ControlPoint6};
for(double t = 3.0; t < 6.0; t += 0.001)
{
const int start = static_cast<int>(t);
BSplineCurve(points[start - 3],
points[start - 2],
points[start - 1],
points[start],
DrawCurve,
t - start);
Draw1Dot(DrawCurve.x, DrawCurve.y, DrawCurve.R, DrawCurve.G, DrawCurve.B);
}
Update
Your B-spline calculation code looks wrong too :-)
bi should be t3/6.0 and not mt3/6.0. See here (slide 25).
The fixed function can look something like this (I did not test it):
void BSplineCurve(const Dot &point1,
const Dot &point2,
const Dot &point3,
const Dot &point4,
const double t,
Dot &result)
{
const double t2 = t * t;
const double t3 = t2 * t;
const double mt = 1.0 - t;
const double mt3 = mt * mt * mt;
const double bi3 = mt3;
const double bi2 = 3 * t3 - 6 * t2 + 4;
const double bi1 =-3 * t3 + 3 * t2 + 3 * t + 1;
const double bi = t3;
result.x = point1.x * bi3 +
point2.x * bi2 +
point3.x * bi1 +
point4.x * bi;
result.x /= 6.0;
result.y = point1.y * bi3 +
point2.y * bi2 +
point3.y * bi1 +
point4.y * bi;
result.y /= 6.0;
}
Maybe the point you use is too very close. In spline it is not good iidea to use very close point. becaue so we have very "galloping" curve. Like this:
red is original
I am just trying to smoothing the image by BiCubic interpolation. I got some code which is used to interpolate the RGB image. I have changed the code to work for Grayscale image. But in result i only got fully black image. Considered input and output image size are same. The code is pasted below. Please help me. Thanks in advance.
inline Uint16 saturate(float x, unsigned max_pixel)
{
return x > max_pixel ? max_pixel
: x < 0.0f ? 0
: Uint16(x);
}
inline float get_subpixel(const Uint16* in, std::size_t dest_width, std::size_t dest_height, unsigned x, unsigned y)
{
if (x < dest_width && y < dest_height)
return in[(y * dest_width) + x];
return 0;
}
void interpolate(unsigned dest_width, unsigned dest_height, unsigned bits_allocated, const Uint16* src, Uint16** dest)
{
const double tx = 1;
const double ty = 1;
float C[5] = { 0 };
unsigned max_bit = pow(2, bits_allocated);
for (unsigned i = 0; i < dest_height; ++i)
{
for (unsigned j = 0; j < dest_width; ++j)
{
const float x = float(tx * j);
const float y = float(ty * i);
const float dx = tx * j - x, dx2 = dx * dx, dx3 = dx2 * dx;
const float dy = ty * i - y, dy2 = dy * dy, dy3 = dy2 * dy;
for (int jj = 0; jj < 4; ++jj)
{
const int idx = y - 1 + jj;
float a0 = get_subpixel(src, dest_width, dest_height, x, idx);
float d0 = get_subpixel(src, dest_width, dest_height, x - 1, idx) - a0;
float d2 = get_subpixel(src, dest_width, dest_height, x + 1, idx) - a0;
float d3 = get_subpixel(src, dest_width, dest_height, x + 2, idx) - a0;
float a1 = -(1.0f / 3.0f) * d0 + d2 - (1.0f / 6.0f) * d3;
float a2 = 0.5f * d0 + 0.5f * d2;
float a3 = -(1.0f / 6.0f) * d0 - 0.5f * d2 + (1.0f / 6.0f) * d3;
C[jj] = a0 + a1 * dx + a2 * dx2 + a3 * dx3;
d0 = C[0] - C[1];
d2 = C[2] - C[1];
d3 = C[3] - C[1];
a0 = C[1];
a1 = -(1.0f / 3.0f) * d0 + d2 - (1.0f / 6.0f) * d3;
a2 = 0.5f * d0 + 0.5f * d2;
a3 = -(1.0f / 6.0f) * d0 - 0.5f * d2 + (1.0f / 6.0f) * d3;
(*dest)[i * dest_width + j] = saturate(a0 + a1 * dy + a2 * dy2 + a3 * dy3, max_bit);
}
}
}
}
How can you have this? The c's havent been computed until the jj loop ends the brace should be above the d's - I'm not considering if the method is correct otherwise.
for (int jj = 0; jj < 4; ++jj)
{
const int idx = y - 1 + jj;
float a0 = get_subpixel(src, dest_width, dest_height, x, idx);
float d0 = get_subpixel(src, dest_width, dest_height, x - 1, idx) - a0;
float d2 = get_subpixel(src, dest_width, dest_height, x + 1, idx) - a0;
float d3 = get_subpixel(src, dest_width, dest_height, x + 2, idx) - a0;
float a1 = -(1.0f / 3.0f) * d0 + d2 - (1.0f / 6.0f) * d3;
float a2 = 0.5f * d0 + 0.5f * d2;
float a3 = -(1.0f / 6.0f) * d0 - 0.5f * d2 + (1.0f / 6.0f) * d3;
C[jj] = a0 + a1 * dx + a2 * dx2 + a3 * dx3;
// } // end jj
d0 = C[0] - C[1];
d2 = C[2] - C[1];
d3 = C[3] - C[1];
a0 = C[1];
a1 = -(1.0f / 3.0f) * d0 + d2 - (1.0f / 6.0f) * d3;
a2 = 0.5f * d0 + 0.5f * d2;
a3 = -(1.0f / 6.0f) * d0 - 0.5f * d2 + (1.0f / 6.0f) * d3;
(*dest)[i * dest_height + j] = saturate(a0 + a1 * dy + a2 * dy2 + a3 * dy3, max_bit);
} // end jj move his above
}
}
I wanted to share great link
cubic splines