float dotProduct(const vec4& v, const vec4& k){
    return v.v[0]*k.v[0] + v.v[1]*k.v[1] + v.v[2] * k.v[2];
}

vec4 crossProduct(const vec4& v, const vec4& k){
    vec4 result(0,0,0,1);
    result.v[0] = (v.v[1]*k.v[2])-(v.v[2]*k.v[1]);
    result.v[1] = -((v.v[0]*k.v[2]) - (v.v[2]*k.v[0]));
    result.v[2] = (v.v[0]*k.v[1]) - (v.v[1]*k.v[0]);

    return result;
}

vec4 floatMultiply(const vec4& v, float f){
    vec4 return_v = v;
    return_v.v[0] *= f;
    return_v.v[1] *= f;
    return_v.v[2] *= f;

    return return_v;
}

vec4 RodriguesRot(const vec4& r, float theta){

        //w a forgatasi tengely
        vec4 w(1,0,0,1);

        float dotResult = dotProduct(w,r);

        vec4 rotated_r = r;
        rotated_r = rotated_r.v[0] * cosf(theta) + rotated_r.v[1] * cosf(theta);

        vec4 temp1 = floatMultiply(floatMultiply(w,dotResult),1-cosf(theta));
        vec4 temp2 = floatMultiply(crossProduct(w,r),sinf(theta));

        rotated_r.v[0] = rotated_r.v[0] + temp1.v[0] + temp2.v[0];
        rotated_r.v[1] = rotated_r.v[1] + temp1.v[1] + temp2.v[1];
        rotated_r.v[2] = rotated_r.v[2] + temp1.v[2] + temp2.v[2];

        return rotated_r;
    }