C++

Forum

 
First Derivative with many variables in C++?

CroCo (201)
Hello all,
I have First derivative function implemented in C++. More details
http://www.cplusplus.com/forum/general/103788/
Special Thank for Andy for helping me to implement it and thank for all of you guys for helping me in the previous link.

In programming, I'm dealing with a lot of variables.
x, y, z & x', y', z' & x'', y'', z'' These are only for one device. The problem is how can I deal with velocities and accelerations the way I did with positions? This is the code

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
 
 while (1)
    {
        /* Perform a synchronous call to copy the most current device state.
           This synchronous scheduler call ensures that the device state
           is obtained in a thread-safe manner. */
        hdScheduleSynchronous(copyDeviceDataCallback, &currentData, HD_MIN_SCHEDULER_PRIORITY);           
		
		
		x_Position[2] =  currentData.m_devicePosition[0];
		x_Position[3] =  currentData.m_devicePosition[0];
		x_Position[4] =  currentData.m_devicePosition[0];

		y_Position[2] =  currentData.m_devicePosition[1];
		y_Position[3] =  currentData.m_devicePosition[1];
		y_Position[4] =  currentData.m_devicePosition[1];

		z_Position[2] =  currentData.m_devicePosition[2];
		z_Position[3] =  currentData.m_devicePosition[2];
		z_Position[4] =  currentData.m_devicePosition[2];

		std::cout << First_Derivative(x_Position) << " " << First_Derivative(y_Position) << " " << First_Derivative(z_Position) << std::endl;

		x_Position[0] =  x_Position[1];
		x_Position[1] =  x_Position[2];

		y_Position[0] =  y_Position[1];
		y_Position[1] =  y_Position[2];

		z_Position[0] =  z_Position[1];
		z_Position[1] =  z_Position[2];

	}



I've tried to create x_Velocity[5] and

1
2
3
4
5
6
7
8
9
10
11
12
13
14
 
x_Position[2] =  currentData.m_devicePosition[0];
		x_Position[3] =  currentData.m_devicePosition[0];
		x_Position[4] =  currentData.m_devicePosition[0];

                x_Velocity[2] = First_Derivative(x_Position);
		x_Velocity[3] = First_Derivative(x_Position);
		x_Velocity[4] = First_Derivative(x_Position);

		
		x_Velocity[1] = x_Velocity[2];
		x_Velocity[0] = x_Velocity[1];

		x_Position[0] =  x_Position[1];  // <----- is affected by calling First_Derivative()
		x_Position[1] =  x_Position[2];  // <----- is affected by calling First_Derivative()


I'm trying also to store x_Velocity in vector. Every time I deal with whether array or vector, the first two points of x position are affected because I'm calling the function for the velocity. Any suggestions?
CroCo (201)
This is my work so far. 

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
 
 
	double x_Position[5] = {0};
	double y_Position[5] = {0};
	double z_Position[5] = {0};

	double x_Velocity[5] = {0};
	double y_Velocity[5] = {0};
	double z_Velocity[5] = {0};
	
	double x_Vel(0);
	double y_Vel(0);
	double z_Vel(0);

	double x_Acceleration(0);
	double y_Acceleration(0);
	double z_Acceleration(0); 

  while (1)
    {
        /* Perform a synchronous call to copy the most current device state.
           This synchronous scheduler call ensures that the device state
           is obtained in a thread-safe manner. */
        hdScheduleSynchronous(copyDeviceDataCallback, &currentData, HD_MIN_SCHEDULER_PRIORITY);           
		
		
		x_Position[2] =  currentData.m_devicePosition[0];
		x_Position[3] =  currentData.m_devicePosition[0];
		x_Position[4] =  currentData.m_devicePosition[0];

		y_Position[2] =  currentData.m_devicePosition[1];
		y_Position[3] =  currentData.m_devicePosition[1];
		y_Position[4] =  currentData.m_devicePosition[1];

		z_Position[2] =  currentData.m_devicePosition[2];
		z_Position[3] =  currentData.m_devicePosition[2];
		z_Position[4] =  currentData.m_devicePosition[2];

		x_Velocity[2] =  x_Vel;
		x_Velocity[3] =  x_Vel;
		x_Velocity[4] =  x_Vel;

		y_Velocity[2] =  y_Vel;
		y_Velocity[3] =  y_Vel;
		y_Velocity[4] =  y_Vel;

		z_Velocity[2] =  z_Vel;
		z_Velocity[3] =  z_Vel;
		z_Velocity[4] =  z_Vel;

		First_Derivative(x_Position, x_Vel);
		First_Derivative(y_Position, y_Vel);
		First_Derivative(z_Position, z_Vel);

		First_Derivative(x_Velocity, x_Acceleration);
		First_Derivative(y_Velocity, y_Acceleration);
		First_Derivative(z_Velocity, z_Acceleration);


		std::cout << "< " <<  x_Acceleration << " , " << y_Acceleration << " , " <<  z_Acceleration << " >" << std::endl;
		
		
		
		x_Position[0] =  x_Position[1];
		x_Position[1] =  x_Position[2];
		
		y_Position[0] =  y_Position[1];
		y_Position[1] =  y_Position[2];

		z_Position[0] =  z_Position[1];
		z_Position[1] =  z_Position[2];

		x_Velocity[0] =  x_Velocity[1];
		x_Velocity[1] =  x_Velocity[2];
		
		y_Velocity[0] =  y_Velocity[1];
		y_Velocity[1] =  y_Velocity[2];

		z_Velocity[0] =  z_Velocity[1];
		z_Velocity[1] =  z_Velocity[2];
}


and this is the First Derivative Function

1
2
3
4
5
6
7
8
9
10
11
12
 
void First_Derivative(double a[5], double& data_dot)
{
    double firstDer(0.0);
    double t(0.001); 
    
    size_t i(2);
    
    firstDer = (   4.0 / 3.0 * (a[i + 1] - a[i - 1]) / (2.0 * t)
                - 1.0 / 3.0 * (a[i + 2] - a[i - 2]) / (4.0 * t) );
   
    data_dot = firstDer;
}


Every time I run the code, I get for the first data of each column the following

1
2
3
4
5
 
0 0 0
0 -2.22918e+007 -2.99833e+007
0 2.86609e+007 3.855e+007
0 -6.82403e+006 -9.17856e+006
0 454935 611904


After that I get the expected data. Why? Also, any suggestions for what I did will be appreciated.
Last edited on
andywestken (4094)
Are you trying to use First_Derivative() to calculate the first few points? Rather than special casing them?

If you are special casing, and something still goes wrong, then please post the corresponding i/p values for the first 10 or so samples.

Andy

PS As you're working with C++, you could make the analysis code a lot simpler by using a 3D "vector" class (you could use std::vector (or std::array, if you're using C++11), but a fixed size, specialized class might be better here.)

You could easily enough implement you own. Or you could use one from a geometry library, like:

Geometric Tools
http://www.geometrictools.com/

Eigen
http://eigen.tuxfamily.org/index.php?title=Main_Page

Game Math and Geometry Library
http://clb.demon.fi/MathGeoLib/nightly/

Or if you were using Qt

QVector3D
http://doc.qt.digia.com/4.6/qvector3d.html

And the Game Math and Geometry Library guys have kindly provided a list of alterntives:

Alternative Libraries
http://clb.demon.fi/MathGeoLib/nightly/alternatives.html
Last edited on
CroCo (201)
@Andy,
God bless you. I really appreciate your help. It seems that I'm not special casing the first data. and this is what I did 

1
2
3
4
5
6
7
8
9
10
11
 
if ( counter > 1 )
{
	First_Derivative(x_Position, x_Vel);
	First_Derivative(y_Position, y_Vel);
	First_Derivative(z_Position, z_Vel);

	First_Derivative(x_Velocity, x_Acceleration);
	First_Derivative(y_Velocity, y_Acceleration);
	First_Derivative(z_Velocity, z_Acceleration);
	xdot << x_Acceleration << " " << y_Acceleration << " " << z_Acceleration << std::endl;
}


It worked but forgive me for this question. Why I should not consider the first points in my calculations since they are initialized as zero. About using vector or array, I thought at the first to use them but they confuse me because I need to use loop to retrieve the data. I'm thinking now to at least run the project then create a function that will be in charge of returning all the variables(x',y',z', x'', etc.) by only accepting stream data(x,y,z) as a parameter. I know it is not well-organized but at least I can track my code easily. Now I'm very close to run the project. Thanks again for your patience.
Last edited on
andywestken (4094)
Why I should not consider the first points in my calculations since they are initialized as zero.

Because the zero values are no where near the real values, so feeding them into the equation that calculates the approximate derivative will generate meaningless values, as you see.

Andy
CroCo (201)
Forgive me but I'm still confused. If my data is zeros, I get zeros for the derivative but what the special with the first points since they are zeros?
Sorry for bothering you.
Thanks again
andywestken (4094)
Could you please post the first 10 or so points of your input data. Plus the corresponding output (i.e. the dirivative calculated)

Andy
Last edited on
CroCo (201)
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
 
        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = -38214.6      y'' = 0
        z = -88.1142       z' = -51399.9      z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 5459.23       y'' = -2.22918e+007
        z = -88.1142       z' = 7342.85       z'' = -2.99833e+007

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 2.86609e+007
        z = -88.1142       z' = 0             z'' = 3.855e+007

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = -6.82403e+006
        z = -88.1142       z' = 0             z'' = -9.17856e+006

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 454935
        z = -88.1142       z' = 0             z'' = 611904
---------------------------------------------------------------------------------------
Expected Results:
--->    x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0

        x = 0              x' = 0             x'' = 0
        y = -65.5107       y' = 0             y'' = 0
        z = -88.1142       z' = 0             z'' = 0
Last edited on
CroCo (201)
I think I see now why this occurs. This what I did based on the results that I got

y = [ 0 0 -65.5 -65.5 -65.5]

y' = 4 * (-65.5-0)/(3*2*0.001) + (65.5-0)*(3*4*0.001)
= -16375


Thanks Andy so much. God bless you
Last edited on
Topic archived. No new replies allowed.