I want to overload the * operator for my QUATERNION class but somehow I'm unable to use it at the same time with QUATERNIONs methods (Q4 example).
Is it possible to use somehow Q1*Q2.conjugated() without creating any temp variables?
C:\Users\xXx\Documents\Arduino\libraries\QUATERNION\extras\Quaternions\main.cpp|19|error:
cannot bind non-const lvalue reference of type 'QUATERNION&' to an
rvalue of type 'QUATERNION'|
QUATERNION Q1(1,2,3,4);
QUATERNION Q2(5,6,7,8);
QUATERNION Q2c = Q2.conjugated();
QUATERNION Q3 = Q1*Q2c; // work fine
QUATERNION Q4 = Q1*Q2.conjugated(); // not working
QUATERNION.h
#ifndef QUATERNION_H
#define QUATERNION_H
class QUATERNION
{
public:
float w;
float x;
float y;
float z;
QUATERNION(void);
QUATERNION(float,float,float,float);
float magnitude(void);
void normalize(void);
void scale(float);
QUATERNION normalized(void);
QUATERNION conjugated(void);
};
// operatory ==========================================
QUATERNION operator * (QUATERNION&,QUATERNION&);
QUATERNION operator * (QUATERNION&,float);
bool operator == (QUATERNION&,QUATERNION&);
// funkcje ============================================
QUATERNION Q_mul(QUATERNION&,QUATERNION&);
QUATERNION Q_scaled(QUATERNION&,float);
QUATERNION Q_fromAngular(const float*);
void Q_toAngular(QUATERNION&,float*,bool);
#endif // QUATERNION_H
QUATERNION.cpp
#include "QUATERNION.h"
#include "math.h"
QUATERNION::QUATERNION() : QUATERNION(1,0,0,0)
{
}
QUATERNION::QUATERNION(float w,float x,float y,float z){
this->w = w;
this->x = x;
this->y = y;
this->z = z;
}
float QUATERNION::magnitude(){
return sqrt(w*w+x*x+y*y+z*z);
}
void QUATERNION::normalize(){
float invMag = 1.0/magnitude();
scale(invMag);
}
void QUATERNION::scale(float scalar){
w *= scalar;
x *= scalar;
y *= scalar;
z *= scalar;
}
QUATERNION QUATERNION::normalized(){
QUATERNION q = QUATERNION(w,x,y,z);
q.normalize();
return q;
}
QUATERNION QUATERNION::conjugated(){
return QUATERNION(w,-x,-y,-z);
}
// operatory ==========================================
QUATERNION operator * (QUATERNION &A,QUATERNION &B){
return Q_mul(A,B);
}
QUATERNION operator * (QUATERNION &q,float scalar){
return Q_scaled(q,scalar);
}
bool operator == (QUATERNION &A,QUATERNION &B){
float epsilon = 1.0e-5;
return fabs(A.w-B.w)<=epsilon && fabs(A.x-B.x)<=epsilon
&& fabs(A.y-B.y)<=epsilon && fabs(A.z-B.z)<=epsilon;
}
// funkcje ============================================
QUATERNION Q_mul(QUATERNION &A,QUATERNION &B){
return QUATERNION(
A.w * B.w - A.x * B.x - A.y * B.y - A.z * B.z, // w
A.w * B.x + A.x * B.w + A.y * B.z - A.z * B.y, // x
A.w * B.y - A.x * B.z + A.y * B.w + A.z * B.x, // y
A.w * B.z + A.x * B.y - A.y * B.x + A.z * B.w); // z
}
QUATERNION Q_scaled(QUATERNION &q,float scalar){
return QUATERNION(q.w*scalar,q.x*scalar,q.y*scalar,q.z*scalar);
}
QUATERNION Q_fromAngular(const float *w) {
float theta,q0,q1,q2,q3;
float dt = 1;
float x = w[0]*dt;
float y = w[1]*dt;
float z = w[2]*dt;
theta = sqrt(x*x + y*y + z*z);
if (theta<=1.0e-6) return QUATERNION(1,0,0,0);
q0 = cos(theta/2.0f);
q1 = sin(theta/2.0f)/theta * x;
q2 = sin(theta/2.0f)/theta * y;
q3 = sin(theta/2.0f)/theta * z;
return QUATERNION(q0,q1,q2,q3);
// w/theta - normalizacja wektora
}
void Q_toAngular(QUATERNION &q,float *angular,bool deg) {
// http://www.euclideanspace.com/physics/kinematics/angularvelocity/index.htm
float w=q.w,x=q.x,y=q.y,z=q.z;
if (w<0){
// w*=-1.0;
// x*=-1.0;
// y*=-1.0;
// z*=-1.0;
}
if (fabs(w)==1){
// unika dzielenia przez 0
// taki kwaternion nie zawiera rotacji
angular[0] = 0;
angular[1] = 0;
angular[2] = 0;
return;
}
// https://math.stackexchange.com/questions/3753611/quaternion-to-rotation-vector-sintheta-2-sqrt1-quaternion-w2
// theta = acos(w)*2
// sqrt(1-w*w) = sin(theta/2)
// float m = ( acos(w)*2.0 )/sqrt(1-w*w); // theta/sin(theta/2)
float theta = 2*acos(w);
float m = theta/sin(theta/2);
if (deg)
m*= 180.0/M_PI;
angular[0] = x*m;
angular[1] = y*m;
angular[2] = z*m;
}
In QUATERNION operator * (QUATERNION& A,QUATERNION& B);, you accept A and B by non-const reference. The compiler expects you to change them. That's obviously wrong for a multiplication. Add the missing const to both arguments, and to all other references in your code.
Also it appears that Angular might be a type. You currently use a float[3] for that.
Related
I'm trying to make a quadcopter drone. I'm using an mpu6050 to get acceleration & angular speed and then convert them to Roll / pitch / yaw
With the acceleration, i'm also trying to get the speed & position by integration. However , i need them to be in absolute coordinate system, because the mpu6050 gives you the values in its relative coordinates system.
the origin of the new coordinate system is the starting position of the drone, and the direction is "where the drone is looking" , we assume Yaw = 0 at the beginning, and we get yaw using the gyrometer's data.
I tried to rotate the vectors using the Roll / pitch values, but that doesn't seem to work very well.
I tried with this being the gravity vector for example : (-2, -2, -1)
If i convert it to the absolute coordinate system, i should get : (0,0, 3)
#include <iostream>
using namespace std;
#include <math.h>
// Vector class, to handle all the vector operations for us
// Thanks to : https://stackoverflow.com/questions/14607640/rotating-a-vector-in-3d-space
class cVector
{
public:
float x;
float y;
float z;
// Constructor
cVector();
cVector(float x1, float y1, float z1);
// returns the vector's magnitude
float Magnitude();
// Normalize ( change length to 1, while keeping the same direction)
void Normalize();
// Rotate around the Axis
void RotateX(float angle);
void RotateY(float angle);
void RotateZ(float angle);
// TODO : Add operators for Addition & Substraction
// Addition
cVector operator+(cVector const& v1) const
{
return cVector(x + v1.x,
y + v1.y,
z + v1.z);
}
void operator+=(cVector const& v1)
{
x += v1.x;
y += v1.y;
z += v1.z;
}
// Substraction
cVector operator-(cVector const& v1) const
{
return cVector(x - v1.x,
y - v1.y,
z - v1.z);
}
void operator-=(cVector const& v1)
{
x -= v1.x;
y -= v1.y;
z -= v1.z;
}
// Multiplication
void operator*=(const float scalar)
{
x *= scalar;
y *= scalar;
z *= scalar;
}
cVector operator*(const float scalar) const
{
return cVector(x * scalar,
y * scalar,
z * scalar);
}
// Division
void operator/=(const float scalar)
{
x /= scalar;
y /= scalar;
z /= scalar;
}
cVector operator/(const float scalar) const
{
return cVector(x / scalar,
y / scalar,
z / scalar);
}
};
// Constructor
cVector::cVector()
{
}
cVector::cVector(float x1, float y1, float z1)
{
x = x1;
y = y1;
z = z1;
}
// returns the vector's magnitude
float cVector::Magnitude()
{
return sqrt((x * x) + (y * y) + (z * z));
}
// Normalize ( change length to 1, while keeping the same direction)
void cVector::Normalize()
{
float flMagnitude = Magnitude();
// We devide the coordinates by the magnitude
x /= flMagnitude;
y /= flMagnitude;
z /= flMagnitude;
}
// Rotate around the Axis
void cVector::RotateX(float angle)
{
// Calculate the sinus and cosinus
float flCos = static_cast<float>(cos(angle));
float flSin = static_cast<float>(sin(angle));
// We save the current values temporarily
float _y = y;
float _z = z;
y = _y * flCos - _z * flSin;
z = _y * flSin + _z * flCos;
}
void cVector::RotateY(float angle)
{
// Calculate the sinus and cosinus
float flCos = static_cast<float>(cos(angle));
float flSin = static_cast<float>(sin(angle));
// We save the current values temporarily
float _x = x;
float _z = z;
x = _x * flCos + _z * flSin;
z = - _x * flSin + _z * flCos;
}
void cVector::RotateZ(float angle)
{
// Calculate the sinus and cosinus
float flCos = static_cast<float>(cos(angle));
float flSin = static_cast<float>(sin(angle));
// We save the current values temporarily
float _x = x;
float _y = y;
x = _x * flCos - _y * flSin;
y = _x * flSin + _y * flCos;
}
void PrintVector(cVector vec)
{
cout << "X : " << vec.x << " Y : " << vec.y << " Z : " << vec.z << endl;
}
// TODO : Add operators for Addition & Substraction
int main()
{
cVector vec(-2, -2, -1);
// Calculate pitch / roll
float pitch = static_cast<float>(atan2( vec.y , sqrt( pow(vec.x,2) + pow(vec.z,2) ) ));
float roll = static_cast<float>(atan2(-1 * vec.x , sqrt( pow(vec.y,2) + pow(vec.z,2) ) ));
// vec.RotateY(1.570796f);
vec.RotateX(roll);
vec.RotateY(pitch);
PrintVector(vec);
cin.get();
return 0;
}
expected result ( 0, 0, 3 )
Actual results : (-0.104919 , -0.824045, -2.8827 )
Closed. This question needs debugging details. It is not currently accepting answers.
Edit the question to include desired behavior, a specific problem or error, and the shortest code necessary to reproduce the problem. This will help others answer the question.
Closed 4 years ago.
Improve this question
I'm trying to add rotation functions to my class, to rotate around the X, Y, Z - axis, but the output is not exactly what i expected
I made sure that my formulas are correct, they seem to be correct, but i don't know. i took them from this : Rotating a Vector in 3D Space
#include <iostream>
using namespace std;
#include <math.h>
// Vector class, to handle all the vector operations for us
// Thanks to : https://stackoverflow.com/questions/14607640/rotating-a-vector-in-3d-space
class cVector
{
public:
float x;
float y;
float z;
// Constructor
cVector();
cVector(float x1, float y1, float z1);
// returns the vector's magnitude
float Magnitude();
// Normalize ( change length to 1, while keeping the same direction)
void Normalize();
// Rotate around the Axis
void RotateX(float angle);
void RotateY(float angle);
void RotateZ(float angle);
// TODO : Add operators for Addition & Substraction
// Addition
cVector operator+(cVector const& v1) const
{
return cVector(x + v1.x,
y + v1.y,
z + v1.z);
}
void operator+=(cVector const& v1)
{
x += v1.x;
y += v1.y;
z += v1.z;
}
// Substraction
cVector operator-(cVector const& v1) const
{
return cVector(x - v1.x,
y - v1.y,
z - v1.z);
}
void operator-=(cVector const& v1)
{
x -= v1.x;
y -= v1.y;
z -= v1.z;
}
// Multiplication
void operator*=(const float scalar)
{
x *= scalar;
y *= scalar;
z *= scalar;
}
cVector operator*(const float scalar) const
{
return cVector(x * scalar,
y * scalar,
z * scalar);
}
// Division
void operator/=(const float scalar)
{
x /= scalar;
y /= scalar;
z /= scalar;
}
cVector operator/(const float scalar) const
{
return cVector(x / scalar,
y / scalar,
z / scalar);
}
};
// Constructor
cVector::cVector()
{
}
cVector::cVector(float x1, float y1, float z1)
{
x = x1;
y = y1;
z = z1;
}
// returns the vector's magnitude
float cVector::Magnitude()
{
return sqrt((x * x) + (y * y) + (z * z));
}
// Normalize ( change length to 1, while keeping the same direction)
void cVector::Normalize()
{
float flMagnitude = Magnitude();
// We devide the coordinates by the magnitude
x /= flMagnitude;
y /= flMagnitude;
z /= flMagnitude;
}
// Rotate around the Axis
void cVector::RotateX(float angle)
{
y = y * cos(angle) - z * sin(angle);
z = y * sin(angle) + z * cos(angle);
}
void cVector::RotateY(float angle)
{
x = (x * cos(angle)) + (z * sin(angle));
z = (-x * sin(angle)) + (z * cos(angle));
}
void cVector::RotateZ(float angle)
{
x = x * cos(angle) - y * sin(angle);
y = x * sin(angle) + y * cos(angle);
}
void PrintVector(cVector vec)
{
cout << "X : " << vec.x << " Y : " << vec.y << " Z : " << vec.z << endl;
}
// TODO : Add operators for Addition & Substraction
int main()
{
cout << "Hello world!" << endl;
cVector vec(10, 0, 0);
vec.RotateZ(1.57f);
PrintVector(vec);
cin.get();
return 0;
}
I expect the method to keep the same magnitude of the vector, and return ( 0, 10, 0) since i'm rotating by pi/2 , but that's not what i'm getting. apparently if i rotate by pi, i get a good result, but other than that, it doesn't work.
First in your Rotation for example in RotateZ you should save the x in some temporary because if you modify it & then try to use it for the y it's obviously gonna cause you an error, ie you should do something like this
void cVector::RotateZ(float angle)
{
float temp = x;
x = x * cos(angle) - y * sin(angle);
y = temp * sin(angle) + y * cos(angle);
}
Second the value of pi you are given is way too over-rounded so the values are false
you can do something like for you pi value
const float Pi = 3.1415926535;
If I try to compile I get the following error:
C2678
binary '-': no operator found which takes a left-hand operand of type 'const D3O::Point' (or there is no acceptable conversion)
code creating error:
forward_list<double> anglelist;
anglelist.resize(pointlist.max_size());
angleto angleTo;
double test = angleTo(pointlist.front(), angleReference);
transform(pointlist.begin(), pointlist.end(), anglelist.begin(),bind2nd(angleTo,angleReference));
Definition of angleto:
struct angleto : public std::binary_function<Point, Vector, const double>
{
const double operator() (Point a, Vector b) const
{ return b.angleTo(a.ToVector());}
};
Definition of angleTo:
const double Vector::angleTo(Vector vec)
{
Vector zVec = this->vecProd(vec);
Vector hVec = this->turnAroundAxisfordeg(zVec, 90);
if (hVec.smallAngle(vec) <= 90)
{
return this->smallAngle(vec);
}
else
{
return (double)(360.0-this->smallAngle(vec));
}
}
const double Vector::smallAngle(Vector vec)
{
if ((this->value() * vec.value()) == 0)
{
return (double)0;
}
else
{
return 180 / M_PI * acos(this->scalar(vec) / (this->value() * vec.value()));
}
}
const double Vector::value()
{
return sqrt(this->X * this->X + this->Y * this->Y + this->Z * this->Z);
}
const Vector Vector::vecProd(Vector vec)
{
return Vector(this->Y * vec.Z - this->Z * vec.Y, this->Z * vec.X - this->X * vec.Z, this->X * vec.Y - this->Y * vec.X);
}
const Vector Vector::turnAroundAxisfordeg(Vector Axis, double degrees)
{
if (this->ColinearTo(Axis))
{
return Vector(this->X,this->Y,this->Z);
}
else
{
double R[3][3] = {};
Vector axis = Axis.getUnitVector();
double deg = degrees / 180 * M_PI;
R[0][0] = axis.X * axis.X * (1 - cos(deg)) + cos(deg); R[0][1] = axis.X * axis.Y * (1 - cos(deg)) - axis.Z * sin(deg); R[0][2] = axis.X * axis.Z * (1 - cos(deg)) + axis.Y * sin(deg);
R[1][0] = axis.Y * axis.X * (1 - cos(deg)) + axis.Z * sin(deg); R[1][1] = axis.Y * axis.Y * (1 - cos(deg)) + cos(deg); R[1][2] = axis.Y * axis.Z * (1 - cos(deg)) - axis.X * sin(deg);
R[2][0] = axis.Z * axis.X * (1 - cos(deg)) - axis.Y * sin(deg); R[2][1] = axis.Z * axis.Y * (1 - cos(deg)) + axis.X * sin(deg); R[2][2] = axis.Z * axis.Z * (1 - cos(deg)) + cos(deg);
double x = this->X * R[0][0] + this->Y * R[0][1] + this->Z * R[0][2];
double y = this->X * R[1][0] + this->Y * R[1][1] + this->Z * R[1][2];
double z = this->X * R[2][0] + this->Y * R[2][1] + this->Z * R[2][2];
x = this->dRound(x, 15);
y = this->dRound(y, 15);
z = this->dRound(z, 15);
return Vector(x, y, z);
}
}
const bool Vector::ColinearTo(Vector vec)
{
return ((this->vecProd(vec)).Value <= 1E-10);
}
const double Vector::scalar(Vector vec)
{
return this->X * vec.X + this->Y * vec.Y + this->Z * vec.Z;
}
const Vector Vector::getUnitVector() {
return Vector(this->X / this->value(), this->Y / this->value(), this->Z / this->value());
}
Definition of ToVector:
const Vector const Point::ToVector() { return Vector(this->X, this->Y, this->Z); }
Why do I get this error? I have included the operator-overwrites in the used namespace.
const Vector const Vector::operator- (const Vector param) { double newX, newY, newZ; newX = X - param.X; newY = Y - param.Y; newZ = Z - param.Z; return Vector(newX, newY, newZ);}
Do I have to pin the class variables X,Y and Z or what is the reason why I get this error? I am really confused, in all functions required to build angleTo, there is no need of any - operator, so why is it complaining about that implementation?
const Vector const Vector::operator- (const Vector param) {
This declares an operator where the left side must be non-const. Ergo:
binary '-': no operator found which takes a left-hand operand of type 'const D3O::Point'.
It can't use that function because the left side is const. I assume you meant to do this instead:
const Vector Vector::operator- (const Vector param) const {
^pointless. remove ^IMPORTANT
So I have code like this:
#include <iostream>
#include <string>
using namespace std;
#pragma region Class Definitions
struct Vector3
{
double x;
double y;
double z;
};
class Plane {
Vector3 point, normal, p_p1, p_p2, p_p3;
public:
Plane(Vector3 p1,
Vector3 p2, Vector3 p3);
const Vector3& get_point() const;
const Vector3& get_normal() const;
double get_hnf_d() const;
Vector3 closest_point(const Vector3& p);
double distance_to(const Vector3& p);
Vector3 intersect_line(const Line &l);
};
int main()
{
a.x = 1;
a.y = 2;
a.z = 3;
b.x = -1;
b.y = 2;
b.z = -2;
s = -2;
o.x = 0;
o.y = 0;
o.z = 0;
p1.x = sqrt(1 / 8);
p1.y = sqrt(1 / 8);
p1.z = sqrt(3 / 4);
p2.x = 0;
p2.y = 2 * sqrt(1 / 8);
p2.z = 0;
p3.x = sqrt(1 / 8) + sqrt(3 / 8);
p3.y = sqrt(1 / 8) + sqrt(3 / 8);
p3.z = sqrt(3 / 4) - sqrt(1 / 4);
q1.x = 1;
q1.y = 1;
q1.z = 1;
q2.x = -1;
q2.y = -1;
q2.z = -1;
switch (choice)
{
...
Plane _plane(p1, p2, p3);
cout << "Distance to p1, p2, p3 respectievly is : " << _plane.distance_to(p1) << endl;
}
return 0;
}
Plane::Plane(Vector3 p1, Vector3 p2, Vector3 p3)
{
p_p1 = p1;
p_p2 = p2;
p_p3 = p3;
}
const Vector3 & Plane::get_point() const
{
return p_p1;
}
const Vector3 & Plane::get_normal() const
{
return normalize(p_p2);
}
double Plane::get_hnf_d() const
{
Vector3 n, numerator;
double denominator;
numerator = cross_product(substract(p_p1, p_p3), substract(p_p2, p_p3));
denominator = sqrt(pow(numerator.x, 2) + pow(numerator.y, 2) + pow(numerator.z, 2));
cout <<"read" << numerator.y << numerator.x << denominator;
n.x = numerator.x / denominator;
n.y = numerator.y / denominator;
n.z = numerator.z / denominator;
//distance from origin
return -1 * dot_product(p_p1, n);
}
Vector3 Plane::closest_point(const Vector3 & p)
{
return Vector3();
}
double Plane::distance_to(const Vector3 & p)
{
return get_hnf_d() + dot_product(p, get_normal()) ;
}
And I am setting the values as such:
Plane::Plane(Vector3 p1, Vector3 p2, Vector3 p3)
{
p_p1 = p1;
p_p2 = p2;
p_p3 = p3;
}
However I always get 0s returned for values whenever I try to say print p_p1.x, thereby failing my computation. where am I wrong? I am really a C++ beginner.
Thanks.
You are doing integer divisions in the line:
p1.x = sqrt(1 / 8);
1/8 is 0 in C++
This is true in all other initialization.
Try writing
p1.x = sqrt(1.0 / 8.0);
at first you must declare in main p1 and p2 and p3 and initialize them then pass them to plane's constructor:
#include <iostream>
using namespace std;
#include <cmath>
struct Vector3
{
double x;
double y;
double z;
};
class Plane
{
Vector3 point, normal, p_p1, p_p2, p_p3;
public:
Plane(Vector3 p1,
Vector3 p2, Vector3 p3) : p_p1(p1), p_p2(p2), p_p3(p3){}
double distance_to(const Vector3& p)const;
};
double Plane:: distance_to(const Vector3& p)const
{
return get_hnf_d() + dot_product(p, get_normal()) ;
}
int main()
{
Vector3 p1 = {sqrt(1.0 / 8.0), sqrt(1.0 / 8.0), sqrt(3.0 / 4.0)};
// do the same thing as above
Vector3 p2, p3;
Plane _plane(p1, p2, p3);
cout << "Distance to p1 is : " << _plane.distance_to(p1) << endl;
return 0;
}
this is just a sample not full code
I am trying to write some position/orientation methods for my small & simple 3d-space calculation library. But I'm stuck on the following problem.
I store 3d line as start and end points. However it should be possible to store it as start point and line's length + orientation as well (it's just a good example to test if orientation calculations works).
By orientation I mean rotation from the initial "0" orientation (which places the end at start + [0,legth,0]). So I first rotate the [0,length,0] by orientation and then add start to it to get end point.
The problem is, my orientation calculations fails somewhere. After calculating the orientation I get different ending point.
I use left-handed coordinate system with Y-axis pointing up, but I don't think it's important here.
Here's the code (I've tried to name the methods in the way you can check if the steps are ok; here's the full source code if you want to compile it yourself):
Point3D start = { 5.0f, 4.0f, 7.0f };
Point3D end = { 15.0f, 6.0f, 14.0f };
Point3D direction = (end - start);
std::wcout << L"Direction: "; direction.output();
float angle = Point3D(0.0f, 1.0f, 0.0f).getAngleToAnotherVectorInRadians(direction);
Point3D axis = direction.getCrossProduct(Point3D(0.0f, 1.0f, 0.0f)).getNormalized();
Quaternion o = Quaternion(AxisAngle(axis, angle));
std::wcout << L"\nAxisAngle: "; AxisAngle(axis, angle).output();
std::wcout << L"\nOrientation: "; o.output();
//test - end2 should be equal to end
Point3D offset(0.0f, (end - start).getLengthAsVector(), 0.0f);
offset = o.rotatePoint(offset);
std::wcout << L"\nOffset: "; offset.output();
Point3D end2 = start + offset;
std::wcout << L"\nEnd2: "; end2.output();
The code produces such output (without a comments, of course):
Direction: {10, 2, 7} //looks ok
AxisAngle: {{-0.573462, 0, 0.819232}, 1.40839}
Orientation: {-0.371272, 0, 0.530388, 0.762132}
Offset: {-10, 2, -7} //Almost! It should be {10, 2, 7}
End2: {-5, 6, -9.53674e-07} //Wrong! It should be { 15, 6, 14 }
In case that all steps are ok but there are some mistakes in methods' implementations I post here the important code for classes (so you can reproduce the problem): Point3D, AxisAngle, Quaternion.
I highly believe that problem(s) lay(s) in my main steps or in AxisAngle calculations. I think that AxisAngle to Quaternion transformation is ok (but I pass the wrong AxisAngle to Quaternion constructor).
The Point3D:
struct Point3D {
protected:
float x, y, z;
public:
Point3D() : x(0.0f), y(0.0f), z(0.0f) {}
Point3D(float x, float y, float z) : x(x), y(y), z(z) {}
void output() { std::wcout << L"{" << x << L", " << y << L", " << z << L"}"; }
Point3D operator-(const Point3D &point) const {
Point3D temp;
temp.setX(getX() - point.getX());
temp.setY(getY() - point.getY());
temp.setZ(getZ() - point.getZ());
return temp;
}
Point3D operator+ (const Point3D &value) const {
Point3D temp;
temp.setX(getX() + value.getX());
temp.setY(getY() + value.getY());
temp.setZ(getZ() + value.getZ());
return temp;
}
inline float getX() const { return x; } inline float getY() const { return y; } inline float getZ() const { return z; }
inline void setX(float x) { this->x = x; } inline void setY(float y) { this->y = y; } inline void setZ(float z) { this->z = z; }
inline float getLengthAsVector() const {
return sqrt(x*x + y*y + z*z);
}
inline Point3D getCrossProduct(const Point3D &anotherVector) const {
//based on: http://www.sciencehq.com/physics/vector-product-multiplying-vectors.html
return Point3D(
y * anotherVector.z - anotherVector.y * z,
z * anotherVector.x - anotherVector.z * x,
x * anotherVector.y - anotherVector.x * y
);
}
inline float getDotProduct(const Point3D &anotherVector) const {
//based on: https://www.ltcconline.net/greenl/courses/107/Vectors/DOTCROS.HTM
return x * anotherVector.x + y * anotherVector.y + z * anotherVector.z;
}
inline float getAngleToAnotherVectorInRadians(const Point3D &anotherVector) const {
//based on: http://math.stackexchange.com/questions/974178/how-to-calculate-the-angle-between-2-vectors-in-3d-space-given-a-preset-function
return acos(getDotProduct(anotherVector) / (getLengthAsVector() * anotherVector.getLengthAsVector()));
}
Point3D getNormalized() const {
float length = std::abs(sqrt(x*x + y*y + z*z));
Point3D result(x / length, y / length, z / length);
return result;
}
};
The AxisAngle:
class AxisAngle {
protected:
Point3D axis;
float angleInRadians;
public:
AxisAngle(const AxisAngle &other) { axis = other.axis; angleInRadians = other.angleInRadians; }
AxisAngle::AxisAngle(float x, float y, float z, float angleInRadians) {
this->axis = Point3D(x, y, z);
this->angleInRadians = angleInRadians;
}
AxisAngle::AxisAngle(const Point3D &axis, float angleInRadians) {
this->axis = axis;
this->angleInRadians = angleInRadians;
}
Point3D getAxis() const { return axis; }
float getAngleInRadians() const { return angleInRadians; }
void output() { std::wcout << L"{"; axis.output(); std::wcout << L", " << angleInRadians << L"}"; }
};
And last but not least, Quaternion:
class Quaternion {
protected:
float x; float y; float z; float w;
public:
Quaternion() { x = 0.0f; y = 0.0f; z = 0.0f; w = 1.0f; }
Quaternion(const Quaternion &other) { x = other.x; y = other.y; z = other.z; w = other.w; }
Quaternion(float x, float y, float z, float w) { this->x = x; this->y = y; this->z = z; this->w = w; }
Quaternion(const AxisAngle &axisAngle) {
Point3D axis = axisAngle.getAxis();
float angleInRadians = axisAngle.getAngleInRadians();
x = sin(angleInRadians / 2) * axis.getX();
y = sin(angleInRadians / 2) * axis.getY();
z = sin(angleInRadians / 2) * axis.getZ();
w = cos(angleInRadians / 2);
normalizeIt();
}
float getLength() const {
return sqrt(x*x + y*y + z*z + w*w);
}
void normalizeIt() {
float length = getLength();
x = x / length;
y = y / length;
z = z / length;
w = w / length;
}
Quaternion getConjugated() const {
return Quaternion(-x, -y, -z, w);
}
Quaternion multiply(Quaternion by) {
//"R" for result
float wR = w * by.getW() - x * by.getX() - y * by.getY() - z * by.getZ();
float xR = x * by.getW() + w * by.getX() + y * by.getZ() - z * by.getY();
float yR = y * by.getW() + w * by.getY() + z * by.getX() - x * by.getZ();
float zR = z * by.getW() + w * by.getZ() + x * by.getY() - y * by.getX();
return Quaternion(xR, yR, zR, wR);
}
//rotate Point3D p around [0,0,0] with this Quaternion
Point3D rotatePoint(Point3D p) const {
Quaternion temp = multiply(p).multiply(getConjugated());
return Point3D(temp.getX(), temp.getY(), temp.getZ());
//G: P' = Q(P-G)Q' + G <- to rotate P around G with Quaternion
}
Quaternion multiply(Point3D r) const {
float wR = -x * r.getX() - y * r.getY() - z * r.getZ();
float xR = w * r.getX() + y * r.getZ() - z * r.getY();
float yR = w * r.getY() + z * r.getX() - x * r.getZ();
float zR = w * r.getZ() + x * r.getY() - y * r.getX();
return Quaternion(xR, yR, zR, wR);
}
inline float getX() const { return x; } inline void setX(float x) { this->x = x; }
inline float getY() const { return y; } inline void setY(float y) { this->y = y; }
inline float getZ() const { return z; } inline void setZ(float z) { this->z = z; }
inline float getW() const { return w; } inline void setW(float w) { this->w = w; }
void output() { std::wcout << L"{" << x << L", " << y << L", " << z << L", " << w << L"}"; }
};
In case somebody would ask: I do want to use quaternions. They may not look 100% needed here, but storing 3d object's orientation as quaternion has many benefits in more complex computations (and most game engines / 3d software use it as well "under the mask").
Your axis has the wrong orientation. It should be:
Point3D axis = Point3D(0.0f, 1.0f, 0.0f).getCrossProduct(direction).getNormalized();
Use the two left-hand rules to figure out the correct order.