4

What am I doing wrong?

In the first print the values are fain, in the second it's all zeros.

The first print - q: 0.965926 0.000000 0.258819 0.000000

The Second print - q: 0.000000 0.000000 0.000000 0.000000

float *AnglesToQaternion(float roll, float pitch, float yaw) { // Convert Euler angles in degrees to quaternions

    static float q[4];

    roll  = roll  * DEG_TO_RAD;
    pitch = pitch * DEG_TO_RAD;
    yaw   = yaw   * DEG_TO_RAD;

    float t0 = cosf(yaw * 0.5);
    float t1 = sinf(yaw * 0.5);
    float t2 = cosf(roll * 0.5);
    float t3 = sinf(roll * 0.5);
    float t4 = cosf(pitch * 0.5);
    float t5 = sinf(pitch * 0.5);

    q[0] = t0 * t2 * t4 + t1 * t3 * t5;
    q[1] = t0 * t3 * t4 - t1 * t2 * t5;
    q[2] = t0 * t2 * t5 + t1 * t3 * t4;
    q[3] = t1 * t2 * t4 - t0 * t3 * t5;

    printf(">> q-1: %f %f %f %f \n", q[0], q[1], q[2], q[3]);

    return q;
}

float q[4] = *AnglesToQaternion(0, 30.0, 0); 
printf(">> q-2: %f %f %f %f \n", q[0], q[1], q[2], q[3]);
Saikrishna Rajaraman
  • 3,205
  • 2
  • 16
  • 29
Gal Dalali
  • 99
  • 1
  • 2
  • 6

6 Answers6

8

Don't return an array at all.

using degree = float;

struct Angles {
    degree roll;
    degree pitch;
    degree yaw;
};

struct Quaternion {
    float i;
    float j;
    float k;
    float l;
};

Quaternion angles_to_quaternion(Angles angles) 
{ 
    float yaw = angles.yaw * DEG_TO_RAD;
    float pitch = angles.pitch * DEG_TO_RAD;
    float roll = angles.roll * DEG_TO_RAD;

    float t0 = cosf(yaw * 0.5);
    float t1 = sinf(yaw * 0.5);
    float t2 = cosf(roll * 0.5);
    float t3 = sinf(roll * 0.5);
    float t4 = cosf(pitch * 0.5);
    float t5 = sinf(pitch * 0.5);

    return {
        t0 * t2 * t4 + t1 * t3 * t5,
        t0 * t3 * t4 - t1 * t2 * t5,
        t0 * t2 * t5 + t1 * t3 * t4,
        t1 * t2 * t4 - t0 * t3 * t5,
    };
}
Caleth
  • 52,200
  • 2
  • 44
  • 75
4

Solution 1: You have to create your array dynamically inside your function if you want to be able to access it from outside. Then you must think to delete it to avoid memory leaks.

float *AnglesToQaternion(float roll, float pitch, float yaw) { // Convert Euler angles in degrees to quaternions

    float *q = new float[4];
    // Or C-style: float *q = malloc(sizeof(float)*4);

    roll  = roll  * DEG_TO_RAD;
    pitch = pitch * DEG_TO_RAD;
    yaw   = yaw   * DEG_TO_RAD;

    float t0 = cosf(yaw * 0.5);
    float t1 = sinf(yaw * 0.5);
    float t2 = cosf(roll * 0.5);
    float t3 = sinf(roll * 0.5);
    float t4 = cosf(pitch * 0.5);
    float t5 = sinf(pitch * 0.5);

    q[0] = t0 * t2 * t4 + t1 * t3 * t5;
    q[1] = t0 * t3 * t4 - t1 * t2 * t5;
    q[2] = t0 * t2 * t5 + t1 * t3 * t4;
    q[3] = t1 * t2 * t4 - t0 * t3 * t5;

    printf(">> q-1: %f %f %f %f \n", q[0], q[1], q[2], q[3]);

    return q;
}

int main() {
    float *q = AnglesToQaternion(0, 30.0, 0); 
    printf(">> q-2: %f %f %f %f \n", q[0], q[1], q[2], q[3]);
    delete[] q;
    // Or C-style: free(q);
}

Solution 2: You can make a static array creation and then pass the address of its first element to your function.

void AnglesToQaternion(float *q, float roll, float pitch, float yaw) { // Convert Euler angles in degrees to quaternions

    roll  = roll  * DEG_TO_RAD;
    pitch = pitch * DEG_TO_RAD;
    yaw   = yaw   * DEG_TO_RAD;

    float t0 = cosf(yaw * 0.5);
    float t1 = sinf(yaw * 0.5);
    float t2 = cosf(roll * 0.5);
    float t3 = sinf(roll * 0.5);
    float t4 = cosf(pitch * 0.5);
    float t5 = sinf(pitch * 0.5);

    q[0] = t0 * t2 * t4 + t1 * t3 * t5;
    q[1] = t0 * t3 * t4 - t1 * t2 * t5;
    q[2] = t0 * t2 * t5 + t1 * t3 * t4;
    q[3] = t1 * t2 * t4 - t0 * t3 * t5;

    printf(">> q-1: %f %f %f %f \n", q[0], q[1], q[2], q[3]);
}

int main() {
    float q[4];
    AnglesToQaternion(&q[0], 0, 30.0, 0); 
    printf(">> q-2: %f %f %f %f \n", q[0], q[1], q[2], q[3]);
}

This second solution is much better for performance (you should avoid new/delete as much as you can if you can).

Benjamin Barrois
  • 2,566
  • 13
  • 30
1

With the minimum change to your code, the function and q should looks like this:

#include <array>

std::array<float, 4> AnglesToQaternion(float roll, float pitch, float yaw)
{
    std::array<float, 4> q{ 0,0,0,0 };

& use the function like this:

std::array<float, 4> q{ 0,0,0,0 };

q = AnglesToQaternion(0, 30.0, 0);
Amit G.
  • 2,546
  • 2
  • 22
  • 30
1

Caleth already wrote the answer that shows the way. But I think you need to start thinking C++ instead of C. Here a complete example:

#define _USE_MATH_DEFINES
#include <cmath>
constexpr float halfDegToRad = 0.5 * M_PI / 180.0;

struct Angles
{
    float roll;
    float pitch;
    float yaw;
};

#include <iostream>
struct Quaternion
{
    float i, j, k, l;

    friend std::ostream& operator<< (std::ostream& os, const Quaternion& q);
};

std::ostream& operator<< (std::ostream& os, const Quaternion& q)
{
    os << q.i << " " << q.j << " " << q.k << " "<< q.l << " ";
    return os;
}

// Convert Euler angles in degrees to quaternions
Quaternion AnglesToQuaternion(const Angles& ang)
{
    float t0 = std::cos(ang.yaw * halfDegToRad);
    float t1 = std::sin(ang.yaw * halfDegToRad);
    float t2 = std::cos(ang.roll * halfDegToRad);
    float t3 = std::sin(ang.roll * halfDegToRad);
    float t4 = std::cos(ang.pitch * halfDegToRad);
    float t5 = std::sin(ang.pitch * halfDegToRad);

    return {
        t0 * t2 * t4 + t1 * t3 * t5,
        t0 * t3 * t4 - t1 * t2 * t5,
        t0 * t2 * t5 + t1 * t3 * t4,
        t1 * t2 * t4 - t0 * t3 * t5 };
}

int main()
{
    Quaternion q = AnglesToQuaternion({ 0.0f, 30.0f, 0.0f });
    std::cout << ">> q-2 : " << q << "\n";
    return 0;
}
JHBonarius
  • 10,824
  • 3
  • 22
  • 41
0

As Mentioned in one of the comment under your question, you cannot really return an array, but a pointer. If you really want it as return value, then make sure you allocate memory in that function for that before using it. Something like:

float *f_array = malloc(sizeof(float)*lengthArray);

An alternative is to pass an array as argument to your function and fill it in the function.

void AnglesToQaternion(float roll, float pitch, float yaw, float resultArray[])

PhoenixBlue
  • 967
  • 2
  • 9
  • 26
  • Thanks, but my main question is how at the end I can do: float q[4] = AnglesToQaternion, I don't want to work with array. – Gal Dalali Jul 31 '18 at 09:48
  • You simply can't. You need to copy content of the array(returned pointer) into the new array. However, you can do `float *q = AnglesToQaternion(...);` Provided that you have allocated memory in the function. – PhoenixBlue Jul 31 '18 at 09:52
0

Pass the address of the local q in main() to AnglesToQaternion for it to populate.

void AnglesToQaternion(float roll, float pitch, float yaw, float * quaternion)