Dinesh Lama - 9 months ago 38

C++ Question

I'm not getting the required output i.e x2,y2,z2 in below code. Here the angle t is in degree, (x2,y2,z2) is the pint of vector face after being rotated and (a2,b2,c2) is the direction cosine of the rotated vector.I used rviz to visualize the rotating vector but its not comming as thought i.e its not rotating.

`double xgp[ipgp] = 23, ygp[ipgp] = 45, zgp[ipgp] = 345; int ipgp = 0;`

double a2, b2 , c2, a1, b1, c1 = ...

double la = 0.0032;

for(double t=0;t<360;t+=la)

{

double x2 = xgp[ipgp]+a2*cos(t)+a1*sin(t);

double y2 = ygp[ipgp]+b2*cos(t)+b1*sin(t);

double z2 = zgp[ipgp]+c2*cos(t)+c1*sin(t);

a2 = x2-xgp[ipgp]; // vector (xgp,ygp,zgp)->(x2,y2,z2)

b2 = y2-ygp[ipgp];

c2 = z2-zgp[ipgp];

int i = round(sqrt(pow(a2,2)+pow(b2,2)+pow(c2,2)));

std::cout<<i<<std::endl;

}

Answer Source

There are two issues with the code that you posted:

- You did not specify how
`(a1,b1,c1)`

and`(a2,b2,c2)`

are set. These need to be:- orthogonal to
`(xgp,ygp,zgp)`

- orthogonal to each other
- unit vectors

- orthogonal to
- In the loop, you reset
`(a2,b2,c2)`

to be the rotated unit vector. You can't do that and still maintain orthogonality between`(a1,b1,c1)`

and`(a2,b2,c2)`

for the next step of the loop. This is probably the main reason why your print out of`i`

is not all`1`

's

The following code does what you intend

```
#include <iostream>
#include <math.h>
int main() {
double xgp = 23., ygp = 45., zgp = 345.;
// setting (a2,b2,c2) to be orthogonal to (xgp,ygp,zgp) with unit magnitude
double a2 = -45./sqrt(23.*23. + 45.*45.);
double b2 = 23./sqrt(23.*23. + 45.*45.);
double c2 = 0.;
// setting (a1,b1,c1) to be orthogonal to both (a2,b2,c2) and (xgp,ygp,zgp)
// using cross product (xgp,ygp,zgp) X (a2,b2,c2) with unit magnitude
double a1 = -23.*345./sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double b1 = -45.*345./sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double c1 = (23.*23. + 45.*45.)/sqrt(23.*345.*23.*345. + 45.*345.*45.*345. + (23.*23. + 45.*45.)*(23.*23. + 45.*45.));
double la = 0.0032;
for(double t=0;t<360;t+=la) {
double x2 = xgp+a2*cos(t)+a1*sin(t);
double y2 = ygp+b2*cos(t)+b1*sin(t);
double z2 = zgp+c2*cos(t)+c1*sin(t);
// cannot overwrite a2, b2, c2; otherwise (a2,b2,c2) is no longer orthogonal to (a1,b1,c1)!
double a3 = x2-xgp; // vector (xgp,ygp,zgp)->(x2,y2,z2)
double b3 = y2-ygp;
double c3 = z2-zgp;
// (a3,b3,c3) is a unit vector!
int i = round(sqrt(pow(a3,2)+pow(b3,2)+pow(c3,2)));
std::cout<<i<<std::endl;
}
return(0)
}
```

Note that rotating in units of degrees or radians does not matter in terms of your question. However, you should follow @Bob 's advice and make sure you know yourself what units you are intending.

Hope this helps.