I'm trying to minimize a following sample function:
F(x) = f[0]^2(x[0],...,x[n-1]) + ... + f[m-1]^2(x[0],...,x[n-1])
A normal way to minimize such a funct could be the Levenberg-Marquardt algorithm.
I would like to perform this minimization in c++ and have done some initial tests
with Eigen that resulted in the expected solution.
My question is the following:
I'm used to optimization in python using i.e. scipy.optimize.fmin_powell. Here
the input function parameters are (func, x0, args=(), xtol=0.0001, ftol=0.0001, maxiter=None, maxfun=None, full_output=0, disp=1, retall=0, callback=None, direc=None).
So I can define a func(x0), give the x0 vector and start optimizing. If needed I can change
the optimization parameters.
Now the Eigen Lev-Marq algorithm works in a different way. I need to define a function
vector (why?) Furthermore I can't manage to set the optimization parameters.
According to:
http://eigen.tuxfamily.org/dox/unsupported/classEigen_1_1LevenbergMarquardt.html
I should be able to use the setEpsilon() and other set functions.
But when I have the following code:
my_functor functor;
Eigen::NumericalDiff<my_functor> numDiff(functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
lm.setEpsilon(); //doesn't exist!
So I have 2 questions:
Why is a function vector needed and why wouldn't a function scalar be enough?
References where I've searched for an answer:
http://www.ultimatepp.org/reference$Eigen_demo$en-us.html
http://www.alglib.net/optimization/levenbergmarquardt.php
How do I set the optimization parameters using the set functions?
So I believe I've found the answers.
1) The function is able to work as a function vector and as a function scalar.
If there are m solveable parameters, a Jacobian matrix of m x m needs to be created or numerically calculated. In order to do a Matrix-Vector multiplication J(x[m]).transpose*f(x[m]) the function vector f(x) should have m items. This can be the m different functions, but we can also give f1 the complete function and make the other items 0.
2) The parameters can be set and read using lm.parameters.maxfev = 2000;
Both answers have been tested in the following example code:
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
// Generic functor
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
typedef _Scalar Scalar;
enum {
InputsAtCompileTime = NX,
ValuesAtCompileTime = NY
};
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
int m_inputs, m_values;
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
int inputs() const { return m_inputs; }
int values() const { return m_values; }
};
struct my_functor : Functor<double>
{
my_functor(void): Functor<double>(2,2) {}
int operator()(const Eigen::VectorXd &x, Eigen::VectorXd &fvec) const
{
// Implement y = 10*(x0+3)^2 + (x1-5)^2
fvec(0) = 10.0*pow(x(0)+3.0,2) + pow(x(1)-5.0,2);
fvec(1) = 0;
return 0;
}
};
int main(int argc, char *argv[])
{
Eigen::VectorXd x(2);
x(0) = 2.0;
x(1) = 3.0;
std::cout << "x: " << x << std::endl;
my_functor functor;
Eigen::NumericalDiff<my_functor> numDiff(functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<my_functor>,double> lm(numDiff);
lm.parameters.maxfev = 2000;
lm.parameters.xtol = 1.0e-10;
std::cout << lm.parameters.maxfev << std::endl;
int ret = lm.minimize(x);
std::cout << lm.iter << std::endl;
std::cout << ret << std::endl;
std::cout << "x that minimizes the function: " << x << std::endl;
std::cout << "press [ENTER] to continue " << std::endl;
std::cin.get();
return 0;
}
This answer is an extension of two existing answers:
1) I adapted the source code provided by #Deepfreeze to include additional comments and two different test functions.
2) I use the suggestion from #user3361661 to rewrite the objective function in the correct form. As he suggested, it reduced the iteration count on my first test problem from 67 to 4.
#include <iostream>
#include <Eigen/Dense>
#include <unsupported/Eigen/NonLinearOptimization>
#include <unsupported/Eigen/NumericalDiff>
/***********************************************************************************************/
// Generic functor
// See http://eigen.tuxfamily.org/index.php?title=Functors
// C++ version of a function pointer that stores meta-data about the function
template<typename _Scalar, int NX = Eigen::Dynamic, int NY = Eigen::Dynamic>
struct Functor
{
// Information that tells the caller the numeric type (eg. double) and size (input / output dim)
typedef _Scalar Scalar;
enum { // Required by numerical differentiation module
InputsAtCompileTime = NX,
ValuesAtCompileTime = NY
};
// Tell the caller the matrix sizes associated with the input, output, and jacobian
typedef Eigen::Matrix<Scalar,InputsAtCompileTime,1> InputType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,1> ValueType;
typedef Eigen::Matrix<Scalar,ValuesAtCompileTime,InputsAtCompileTime> JacobianType;
// Local copy of the number of inputs
int m_inputs, m_values;
// Two constructors:
Functor() : m_inputs(InputsAtCompileTime), m_values(ValuesAtCompileTime) {}
Functor(int inputs, int values) : m_inputs(inputs), m_values(values) {}
// Get methods for users to determine function input and output dimensions
int inputs() const { return m_inputs; }
int values() const { return m_values; }
};
/***********************************************************************************************/
// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Booth Function
// Implement f(x,y) = (x + 2*y -7)^2 + (2*x + y - 5)^2
struct BoothFunctor : Functor<double>
{
// Simple constructor
BoothFunctor(): Functor<double>(2,2) {}
// Implementation of the objective function
int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const {
double x = z(0); double y = z(1);
/*
* Evaluate the Booth function.
* Important: LevenbergMarquardt is designed to work with objective functions that are a sum
* of squared terms. The algorithm takes this into account: do not do it yourself.
* In other words: objFun = sum(fvec(i)^2)
*/
fvec(0) = x + 2*y - 7;
fvec(1) = 2*x + y - 5;
return 0;
}
};
/***********************************************************************************************/
// https://en.wikipedia.org/wiki/Test_functions_for_optimization
// Himmelblau's Function
// Implement f(x,y) = (x^2 + y - 11)^2 + (x + y^2 - 7)^2
struct HimmelblauFunctor : Functor<double>
{
// Simple constructor
HimmelblauFunctor(): Functor<double>(2,2) {}
// Implementation of the objective function
int operator()(const Eigen::VectorXd &z, Eigen::VectorXd &fvec) const {
double x = z(0); double y = z(1);
/*
* Evaluate Himmelblau's function.
* Important: LevenbergMarquardt is designed to work with objective functions that are a sum
* of squared terms. The algorithm takes this into account: do not do it yourself.
* In other words: objFun = sum(fvec(i)^2)
*/
fvec(0) = x * x + y - 11;
fvec(1) = x + y * y - 7;
return 0;
}
};
/***********************************************************************************************/
void testBoothFun() {
std::cout << "Testing the Booth function..." << std::endl;
Eigen::VectorXd zInit(2); zInit << 1.87, 2.032;
std::cout << "zInit: " << zInit.transpose() << std::endl;
Eigen::VectorXd zSoln(2); zSoln << 1.0, 3.0;
std::cout << "zSoln: " << zSoln.transpose() << std::endl;
BoothFunctor functor;
Eigen::NumericalDiff<BoothFunctor> numDiff(functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<BoothFunctor>,double> lm(numDiff);
lm.parameters.maxfev = 1000;
lm.parameters.xtol = 1.0e-10;
std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
std::cout << "x tol: " << lm.parameters.xtol << std::endl;
Eigen::VectorXd z = zInit;
int ret = lm.minimize(z);
std::cout << "iter count: " << lm.iter << std::endl;
std::cout << "return status: " << ret << std::endl;
std::cout << "zSolver: " << z.transpose() << std::endl;
std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}
/***********************************************************************************************/
void testHimmelblauFun() {
std::cout << "Testing the Himmelblau function..." << std::endl;
// Eigen::VectorXd zInit(2); zInit << 0.0, 0.0; // soln 1
// Eigen::VectorXd zInit(2); zInit << -1, 1; // soln 2
// Eigen::VectorXd zInit(2); zInit << -1, -1; // soln 3
Eigen::VectorXd zInit(2); zInit << 1, -1; // soln 4
std::cout << "zInit: " << zInit.transpose() << std::endl;
std::cout << "soln 1: [3.0, 2.0]" << std::endl;
std::cout << "soln 2: [-2.805118, 3.131312]" << std::endl;
std::cout << "soln 3: [-3.77931, -3.28316]" << std::endl;
std::cout << "soln 4: [3.584428, -1.848126]" << std::endl;
HimmelblauFunctor functor;
Eigen::NumericalDiff<HimmelblauFunctor> numDiff(functor);
Eigen::LevenbergMarquardt<Eigen::NumericalDiff<HimmelblauFunctor>,double> lm(numDiff);
lm.parameters.maxfev = 1000;
lm.parameters.xtol = 1.0e-10;
std::cout << "max fun eval: " << lm.parameters.maxfev << std::endl;
std::cout << "x tol: " << lm.parameters.xtol << std::endl;
Eigen::VectorXd z = zInit;
int ret = lm.minimize(z);
std::cout << "iter count: " << lm.iter << std::endl;
std::cout << "return status: " << ret << std::endl;
std::cout << "zSolver: " << z.transpose() << std::endl;
std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
}
/***********************************************************************************************/
int main(int argc, char *argv[])
{
std::cout << "~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~" << std::endl;
testBoothFun();
testHimmelblauFun();
return 0;
}
The output at the command line from running this test script is:
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Testing the Booth function...
zInit: 1.87 2.032
zSoln: 1 3
max fun eval: 1000
x tol: 1e-10
iter count: 4
return status: 2
zSolver: 1 3
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
Testing the Himmelblau function...
zInit: 1 -1
soln 1: [3.0, 2.0]
soln 2: [-2.805118, 3.131312]
soln 3: [-3.77931, -3.28316]
soln 4: [3.584428, -1.848126]
max fun eval: 1000
x tol: 1e-10
iter count: 8
return status: 2
zSolver: 3.58443 -1.84813
~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~
As an alternative you may simply create a new functor like this,
struct my_functor_w_df : Eigen::NumericalDiff<my_functor> {};
and then initialize the LevenbergMarquardt instance using like this,
my_functor_w_df functor;
Eigen::LevenbergMarquardt<my_functor_w_df> lm(functor);
Personally, I find this approach a bit cleaner.
It seems that the function is more general:
Let's say you have an m parameter model.
You have n observations to which you want to fit the m-parameter model in a least-squares sense.
The Jacobian, if provided, will be n times m.
You will need to supply n error values in the fvec.
Also, there is no need to square the f-values because it is implicitly assumed that the overall error function is made up of the sum of squares of the fvec components.
So, if you follow these guidelines and change the code to:
fvec(0) = sqrt(10.0)*(x(0)+3.0);
fvec(1) = x(1)-5.0;
It will converge in a ridiculously small number of iterations - like less than 5. I also tried it on a more complex example - the Hahn1 benchmark at http://www.itl.nist.gov/div898/strd/nls/data/hahn1.shtml with m=7 parameters and n=236 observations and it converges to the known right solution in only 11 iterations with the numerically computed Jacobian.
Related
I need to pass structure to the function Constraint::AddFixedOrientationAxis, however when I check the passed data their values are completely wrong. I have tried to use different datatypes but without any luck.
typedef struct{
size_t idx;
size_t axis_idx;
double axis_vector_1;
double axis_vector_2;
double axis_vector_3;
}AddFixedOrientationAxisData;
double Constraint::AddFixedOrientationAxis(const std::vector<double> &x, std::vector<double> &grad, void *data)
{
Eigen::VectorXd fixed_rot(3);
AddFixedOrientationAxisData *d = reinterpret_cast<AddFixedOrientationAxisData*>(data);
auto idx = d->idx;
auto axis_idx = d->axis_idx; // 0->x, 1->y, 2->z
fixed_rot << d->axis_vector_1, d->axis_vector_2, d->axis_vector_3;
std::cout << "idx: " << idx << std::endl;
std::cout << "axis: " << axis_idx << std::endl;
std::cout << "fixed_rot: " << fixed_rot << std::endl;
}
In the main, I call use it the same way as the tutorial is:
AddFixedOrientationAxisData fixed_orient_constraint_data;
fixed_orient_constraint_data.idx = 0;
fixed_orient_constraint_data.axis_idx = 0;
fixed_orient_constraint_data.axis_vector_1 = FK_q(0,0);
fixed_orient_constraint_data.axis_vector_2 = FK_q(1,0);
fixed_orient_constraint_data.axis_vector_3 = FK_q(2,0);
opt.add_equality_constraint(Constraint::AddFixedOrientationAxis, &fixed_orient_constraint_data);
The terminal output is:
idx: 93901286131024
axis: 93901286131080
fixed_rot:
4.63934e-310
-0.54938 //interestingly, this is the correct value
0.00838157 //interestingly, this is the correct value
As #{Some programmer dude} told me in the comments, the problem was that the variable was not alive when the function was called.
This question already has answers here:
problem sorting using member function as comparator
(9 answers)
Closed 2 years ago.
I'm trying to implement this code for Linear Regression here but it is unable to compile due to std::sort returning several errors.
#include "LinearRegression.h"
#include <iostream>
#include <algorithm>
#include <vector>
bool LinearRegression::custom_sort(double a, double b) /*sorts based on absolute min value or error*/
{
double a1 = abs(a-0);
double b1 = abs(b-0);
return a1<b1;
}
void LinearRegression::predict()
{
/*Intialization Phase*/
double x[] = { 1, 2, 4, 3, 5 }; //defining x values
double y[] = { 1, 3, 3, 2, 5 }; //defining y values
double err;
double b0 = 0; //initializing b0
double b1 = 0; //initializing b1
double alpha = 0.01; //intializing error rate
std::vector<double>error; // array to store all error values
/*Training Phase*/
for (int i = 0; i < 20; i++) //
{
int idx = i % 5; //for accessing index after every epoch
double p = b0 + b1 * x[idx]; //calculate prediction
err = p - y[idx]; //calculate error
b0 = b0 - alpha * err; //update b0
b1 = b1 - alpha * err * x[idx]; // updating b1
std::cout << "B0=" << b0 << " " << "B1=" << b1 << " " << "error=" << err << std::endl; //print values after every update
error.push_back(err);
}
std::sort(error.begin(),error.end(),custom_sort); //sorting based on error values
std::cout << "Final Values are: " << "B0=" << b0 << " " << "B1=" << b1 << " " << "error=" << error[0] << std::endl;
/*Testing Phase*/
std::cout << "Enter a test x value";
double test;
std::cin >> test;
double pred = b0 + b1 * test;
std::cout << std::endl;
std::cout << "The value predicted by the model= " << pred;
}
I get the errors:
non-standard syntax; use '&' to create a pointer to member
and
no matching overloaded function found
for this line
std::sort(error.begin(),error.end(),custom_sort);
custom_sort is a non-static member function, it can't work with sort directly because under the hood it expects three arguments, not two - the first one is the object pointer.
In your particular example, custom_sort doesn't use any data members, so it can be made static (static should go into the declaration):
static bool LinearRegression::custom_sort(double a, double b);
Then you could write:
std::sort(error.begin(), error.end(), &custom_sort);
Note that you need & to form a pointer to a member function, as the error message suggests. However, for static member function it can be omitted (similar to free functions).
I cannot seem to understand why passing directly a function as argument to ublas::element_prod() produces a wrong result.
If I run the following code:
#include <cmath>
#include <boost/numeric/ublas/vector.hpp>
#include <boost/numeric/ublas/io.hpp>
namespace ublas = boost::numeric::ublas;
ublas::vector<double>
vector_ln(ublas::vector<double> x) {
for(auto it = x.begin(); it != x.end(); ++it) {
(*it) = log(*it);
}
return x;
}
int main(void) {
ublas::vector<double> x(2, 2.0);
ublas::vector<double> y(2, 10.0);
std::cout << "x = " << x << std::endl;
std::cout << "y = " << y << std::endl;
auto tmp = vector_ln(y);
auto ret1 = ublas::element_prod(
x,
tmp);
std::cout << ret1 << std::endl;
std::cout << "x = " << x << std::endl;
std::cout << "y = " << y << std::endl;
auto ret2 = ublas::element_prod(
x,
vector_ln(y));
std::cout << ret2 << std::endl;
}
I get the following output:
x = [2](2,2)
y = [2](10,10)
[2](4.60517,4.60517)
x = [2](2,2)
y = [2](10,10)
[2](0,4.60517)
Can anybody enlighten me on why the second coding style produces a wrong result, with no compile error?
The problem is that ublas uses Expression templates, where the result of a number of operations is a temporary which simply keeps a pointer/reference to its inputs, and is evaluated when assigning to a variable. This is done to reduce unnecessary computations and copies. See https://en.wikipedia.org/wiki/Expression_templates
However, with the introduction of C++11, it creates a dangerous interaction with the use of auto, as this saves a copy of the expression template, and not the result. This expression template has dangling references to the temporary returned by vector_ln(y), and causes the problem you are seeing.
Since the primary problem is the interaction with auto, the solution is to save it to the correct ublas matrix type as the result of element_prod(). It only worked in the first case because none of the stored references were temporaries.
I was using a templatized meter function (see below) to measure the elapsed time for functions.
Then I also wanted to use it for constructors.
As far as I know there is no way of passing a type as a function parameter directly. So I came up with this workaround, passing it solely as the template parameter (minimal example):
template <typename T, typename ... P>
auto meter(T t, P ... p) {
auto t1 = high_resolution_clock::now();
t(p...);
auto t2 = high_resolution_clock::now();
auto dif = t2-t1;
return duration_cast<microseconds>(dif);
}
template <typename T, typename ... P>
auto meter(P ... p) {
auto t1 = high_resolution_clock::now();
auto t = T(p...);
auto t2 = high_resolution_clock::now();
auto dif = t2-t1;
return duration_cast<microseconds>(dif);
}
int main() {
auto d = meter(g, 1.0, 20.0); //meter the function call g(1.0, 20.0)
std::cout << "Ellapsed time: " << d.count() << " microseconds\n";
d = meter(complex_obj{2}); //meter () operator of complex_obj, assuming complex_obj{int} is trivial;
std::cout << "Ellapsed time: " << d.count() << " microseconds\n";
d = meter<complex_obj>(); //meter constructor complex_obj();
std::cout << "Ellapsed time: " << d.count() << " microseconds\n";
}
Trying this got me thinking. Is there a general/consistent way of rewriting this, as to apply to any kind of computation (not just constructor, but maybe even other operators like (obj1 < obj2)? I noticed, that I was already (accidentally) supporting the () operator of structs.
Sorry if this question became to broad, my main question is, if there is a way to unite the syntax of meter call, for functions and constructors alike.
You can wrap the code to be measured inside a lambda (since C++11):
#include <chrono>
#include <iostream>
template<class F>
auto meter(F&& f) {
auto t1 = std::chrono::high_resolution_clock::now();
f();// <-- operator() of the lambda
auto t2 = std::chrono::high_resolution_clock::now();
auto dif = t2-t1;
return std::chrono::duration_cast<std::chrono::microseconds>(dif);
}
void g(double x, double y) {
std::cout << "g(" << x << ", " << y << ")\n";
}
int main() {
double x = 1.0;
auto d = meter([&] {
// This comment is inside the *body* of the lambda.
// Code of the {body} is executed upon `operator()`.
g(x, 20.0);// note that you can use `x` here thanks to the capture-default `[&]`
});
std::cout << "time: " << d.count() << " ms\n";
}
However you encapsulate the actual function call, it'll probably be for the best if you make your meter function return the value returned by the function it's measuring to make it possible to chain calls - but still with the possibility to check how long time each individual call took afterwards. This makes it theoretically possible for RVO / copy elision to kick in and therefor not slow down the code as much. Example:
#include <chrono>
#include <iostream>
#include <thread> // for debug sleeps only
using namespace std::chrono;
template<typename D, typename F, typename... P>
auto meter(D& dur, F func, P&&... params) {
auto start = high_resolution_clock::now();
auto retval = func(std::forward<P>(params)...);
// put duration in the duration reference
dur = duration_cast<D>(high_resolution_clock::now() - start);
// and return func()'s return value
return retval;
}
namespace m {
double add(double a, double b) {
std::this_thread::sleep_for(milliseconds(10));
return a + b;
}
double sub(double a, double b) {
std::this_thread::sleep_for(milliseconds(11));
return a - b;
}
double mul(double a, double b) {
std::this_thread::sleep_for(milliseconds(12));
return a * b;
}
double div(double a, double b) {
std::this_thread::sleep_for(milliseconds(13));
return a / b;
}
} // namespace m
int main() {
milliseconds Add, Sub, Mul, Div;
// chaining calls for this calculation:
// (1000 / (100 * (4.3 - (1.1+2.2))))
auto result = meter(Div, m::div,
1000.0, meter(Mul, m::mul,
100.0, meter(Sub, m::sub,
4.3, meter(Add, m::add,
1.1, 2.2)
)
)
);
std::cout << "Add: " << Add.count() << " ms.\n";
std::cout << "Sub: " << Sub.count() << " ms.\n";
std::cout << "Mul: " << Mul.count() << " ms.\n";
std::cout << "Div: " << Div.count() << " ms.\n";
std::cout << result << "\n";
}
Probable output:
Add: 10 ms.
Sub: 11 ms.
Mul: 12 ms.
Div: 13 ms.
10
This I feel is a rather complicated problem, I hope I can fit it in to small enough of a space to make it understandable. I'm presently writing code to
simulate Ideal gas particles inside a box. I'm calculating if two particles will collide having calculated the time taken for them to reach their closest point. (using an example where they have head on collision).
In this section of code I need to find if they will collide at all for two particles, before then calculating at what time and how they collide etc.
Thus for my two paricles:
Main.cpp
Vector vp1(0,0,0);
Vector vv1(1,0,0);
Vector vp2(12,0,0);
Vector vv2(-1,0,0);
Particle Particle1(1, vp1, vv1);
Particle Particle2(1, vp2, vv2);
Particle1.timeToCollision(Particle2);
Within my program I define a particle to be:
Header File
class Particle {
private:
Vector p; //position
Vector v; //velocity
double radius; //radius
public:
Particle();
Particle(double r, const Vector Vecp, const Vector Vecv);
void setPosition(Vector);
void setVelocity(Vector);
Vector getPosition() const;
Vector getVelocity() const;
double getRadius() const;
void move(double t);
double timeToCollision(const Particle particle);
void collideParticles(Particle);
~Particle();
};
Vector is another class that in short gives x, y, z values. It also contains multiple functions for manipulating these.
And the part that I need help with, within the .cpp (Ignore the cout start and letters etc, they are simple checks where my code exits for tests.)
Given the equations:
I have already written code to do the dot product and modulus for me and:
where
s is distance travelled in time tac.
double Particle::timeToCollision(const Particle particle){
Vector r2 = particle.getPosition();
Vector r1 = p;
Vector v2 = particle.getVelocity();
Vector v1 = v;
Vector r0 = r2 - r1;
Vector v = v2 - v1;
double modv;
double tca;
double result = 0;
double bsqr;
modv = getVelocity().modulus();
cout << "start" << endl;
if(modv < 0.0000001){
cout << "a" << endl;
result = FLT_MAX;
}else{
cout << "b" << endl;
tca = ((--r0).dot(v)) / v.modulusSqr();
// -- is an overridden operator that gives the negation ( eg (2, 3, 4) to (-2, -3, -4) )
if (tca < 0) {
cout << "c" << endl;
result = FLT_MAX;
}else{
cout << "d" << endl;
Vector s(v.GetX(), v.GetY(), v.GetZ());
s.Scale(tca);
cout << getVelocity().GetX() << endl;
cout << getVelocity().GetY() << endl;
cout << getVelocity().GetZ() << endl;
double radsqr = radius * radius;
double bx = (r0.GetX() * r0.GetX() - (((r0).dot(v)) *((r0).dot(v)) / v.modulusSqr()));
double by = (r0.GetY() * r0.GetY() - (((r0).dot(v)) *((r0).dot(v)) / v.modulusSqr()));
double bz=(r0.GetZ() * r0.GetZ() - (((r0).dot(v)) * ((r0).dot(v)) / v.modulusSqr()));
if (bsqr < 4 * radsqr) {
cout << "e" << endl;
result = FLT_MAX;
} else {
}
cout << "tca: " << tca << endl;
}
}
cout << "fin" << endl;
return result;
}
I have equations for calculating several aspects, tca refers to Time of closest approach.
As written in the code I need to check if b > 4 r^2, I Have made some attempts and written the X, Y and Z components of b out. But I'm getting rubbish answers.
I just need help to establish if I've already made mistakes or the sort of direction I should be heading.
All my code prior to this works as expected and I've written multiple tests for each to check.
Please inform me in a comment for any information you feel I've left out etc.
Any help greatly appreciated.
You had several mistakes in your code. You never set result to a value different from 0 or FLT_MAX. You also never calculate bsqr. And I guess the collision happens if bsqr < 4r^2 and not the other way round. (well i do not understand why 4r^2 instead of r^2 but okay). Since you hide your vector implementation I used a common vector library. I also recommend to not use handcrafted stuff anyway. Take a look into armadillo or Eigen.
Here you go with a try in Eigen.
#include <iostream>
#include <limits>
#include <type_traits>
#include "Eigen/Dense"
struct Particle {
double radius;
Eigen::Vector3d p;
Eigen::Vector3d v;
};
template <class FloatingPoint>
std::enable_if_t<std::is_floating_point<FloatingPoint>::value, bool>
almost_equal(FloatingPoint x, FloatingPoint y, unsigned ulp=1)
{
FloatingPoint max = std::max(std::abs(x), std::abs(y));
return std::abs(x-y) <= std::numeric_limits<FloatingPoint>::epsilon()*max*ulp;
}
double timeToCollision(const Particle& left, const Particle& right){
Eigen::Vector3d r0 = right.p - left.p;
Eigen::Vector3d v = right.v - left.v;
double result = std::numeric_limits<double>::infinity();
double vv = v.dot(v);
if (!almost_equal(vv, 0.)) {
double tca = (-r0).dot(v) / vv;
if (tca >= 0) {
Eigen::Vector3d s = tca*v;
double bb = r0.dot(r0) - s.dot(s);
double radius = std::max(left.radius, right.radius);
if (bb < 4*radius*radius)
result = tca;
}
}
return result;
}
int main()
{
Eigen::Vector3d vp1 {0,0,0};
Eigen::Vector3d vv1 {1,0,0};
Eigen::Vector3d vp2 {12,0,0};
Eigen::Vector3d vv2 {-1,0,0};
Particle p1 {1, vp1, vv1};
Particle p2 {1, vp2, vv2};
std::cout << timeToCollision(p1, p2) << '\n';
}
My apologies for a very poorly worded question that was to long and bulky to make much sense of. Luckily I have found my own answer to be much easier then initially anticipated.
double Particle::timeToCollision(const Particle particle){
Vector r2=particle.getPosition();
Vector r1=p;
Vector v2=particle.getVelocity();
Vector v1=v;
Vector r0=r2-r1;
Vector v=v2-v1;
double modv;
double tca = ((--r0).dot(v)) / v.modulusSqr();
double bsqr;
double result=0;
double rColTestx=r0.GetX()+v.GetX()*tca;
double rColTesty=r0.GetY()+v.GetY()*tca;
double rColTestz=r0.GetZ()+v.GetZ()*tca;
Vector rtColTest(rColTestx, rColTesty, rColTestz);
modv=getVelocity().modulus();
cout << "start " << endl;
if(modv<0.0000001){
cout << "a" << endl;
result=FLT_MAX;
}else{
cout << "b" << endl;
if (tca < 0) {
cout << "c" << endl;
result=FLT_MAX;
}else{
cout << "d" << endl;
Vector s(v.GetX(), v.GetY(), v.GetZ());
s.Scale(tca);
cout << getVelocity().GetX() << endl;
cout << getVelocity().GetY() << endl;
cout << getVelocity().GetZ() << endl;
double radsqr= radius*radius;
bsqr=rtColTest.modulusSqr();
if (bsqr < 4*radsqr) {
cout << "e" << endl;
cout << "collision occurs" << endl;
result = FLT_MAX;
} else {
cout << "collision does not occurs" << endl;
}
}
}
cout << "fin" << endl;
return result;
}
Sorry its a large section of code. Also FLT_MAX is from the cfloat lib. I didn't stat this in my question. I found this to work for several examples I calculated on paper to check.
To be Clear, the return resultand result=0 were arbitrary. I later edit to return time but for this part didn't need or want that.