Integrating Circular Motion Estimates in 2D

1. Theory

2. Example
Circular motion start at (Px₁,Py₁,theta₁) = (5,4,128.6598^o) with 30^o angular displacement.

3. Code

#include<iostream>

#include <iomanip>

#include <math.h>

#define PI 3.14159265

using namespace std;

int main ()

{     

       double motion0[4] = {5,4,128.6598,30};// Example Starting :{Px1,Py1,theta1,dtime}

       double dtheta = 30 * (PI/180);

       double radius = 6.4031;

       double motion1[4] ; // {Px2,Py2,theta2,dtime}

      

       cout << motion0[0] << ";" << motion0[1] << ";" << motion0[2] << endl;

      

motion0[2] = motion0[2] * (PI/180); //Change to radians

for (int i = 0; i < 20; i++)

       {            

motion1[0] = motion0[0] + radius *( sin(dtheta+motion0[2]) - sin(motion0[2]) ); // Equation of Xnew      

motion1[1] = motion0[1] - radius *( cos(dtheta+motion0[2]) - cos(motion0[2]) ); // Equation of Ynew

              motion1[2] = motion0[2] + dtheta; // Equation of thetanew

             

cout << motion1[0] << ";" << motion1[1] << ";" << motion1[2]*(180/PI) << endl;  

              motion0[0] = motion1[0]; motion0[1] = motion1[1];motion0[2]=motion1[2];   

              //Replace Px1,Py1 and theta1 with Xnew, Ynew and thetanew

                           

}     

       system("PAUSE");

       return 0;

}

4. Result