I want to rotate a point around the origin but I keep getting errors. The mathematical part is solid but the code seems to fail when I want it to update the x and y values from the specific points. Can you guys help me?
Kind regards,
Vincent
#include <cmath>
#include <iostream>
#include <iomanip>
class Point
{
public:
double x, y;
// constructors
Point()
: x(0), y(0)
{}
Point(double X, double Y)
: x(X), y(Y)
{}
double roa(double angle)
{
double new_x = x*cos(angle) - y*sin(angle);
double new_y = x*sin(angle) + y*sin(angle);
x = new_x;
y = new_y;
return Point(x,y);
}
};
int main()
{
Point a(2,2);
a = a.roa(50);
std::cout << a << std::endl;
return 0
}
Solved! Thx for your help guys. You can find the new code below:
#include <cmath>
#include <iostream>
#include <iomanip>
class Point
{
public:
double x, y;
// constructors
Point()
: x(0), y(0)
{}
Point(double X, double Y)
: x(X), y(Y)
{}
Point roa(double angle)
{
double angle_rad = angle / (180/M_PI);
double new_x = x*cos(angle_rad) - y*sin(angle_rad);
double new_y = x*sin(angle_rad) + y*cos(angle_rad);
double x = new_x;
double y = new_y;
Point p;
p.x = new_x;
p.y = new_y;
return p;
}
};
int main()
{
Point a(2,2);
a = a.roa(360);
std::cout << a << std::endl;
return 0
}
A few issues in the code. You are returning Point(x, y) on roa() while the function returns a double which makes it unable to compile. If you want to rotate the same point, you are already setting x and y values in roa(), no need to reassign the whole variable at a = a.roa(50). Just do a.roa() with roa() modified as follows:
void roa(double angle)
{
double new_x = x*cos(angle) - y*sin(angle);
double new_y = x*sin(angle) + y*cos(angle); // mistake here as well
x = new_x;
y = new_y;
}
Finally, as bialy pointed out, the angle should be in radians not degrees!
Documentation says that angle is in radians:
http://www.cplusplus.com/reference/cmath/cos/
To convert angles to radians just
double angle_radians = angle_degrees / (180.0 / M_PI);
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;
Is there any way I can modify the poisson-disk points generator finding here.I need to generate new poisson points using the coordinates of points in the textfile.txt to improve the distribution. below the c++ code of poisson-disk sampling in a unit square.
poissonGenerator.h:
#include <vector>
#include <random>
#include <stdint.h>
#include <time.h>
namespace PoissoGenerator
{
class DefaultPRNG
{
public:
DefaultPRNG()
: m_Gen(std::random_device()())
, m_Dis(0.0f, 1.f)
{
// prepare PRNG
m_Gen.seed(time(nullptr));
}
explicit DefaultPRNG(unsigned short seed)
: m_Gen(seed)
, m_Dis(0.0f, 1.f)
{
}
double RandomDouble()
{
return static_cast <double>(m_Dis(m_Gen));
}
int RandomInt(int Max)
{
std::uniform_int_distribution<> DisInt(0, Max);
return DisInt(m_Gen);
}
private:
std::mt19937 m_Gen;
std::uniform_real_distribution<double> m_Dis;
};
struct sPoint
{
sPoint()
: x(0)
, y(0)
, m_valid(false){}
sPoint(double X, double Y)
: x(X)
, y(Y)
, m_valid(true){}
double x;
double y;
bool m_valid;
//
bool IsInRectangle() const
{
return x >= 0 && y >= 0 && x <= 1 && y <= 1;
}
//
bool IsInCircle() const
{
double fx = x - 0.5f;
double fy = y - 0.5f;
return (fx*fx + fy*fy) <= 0.25f;
}
};
struct sGridPoint
{
sGridPoint(int X, int Y)
: x(X)
, y(Y)
{}
int x;
int y;
};
double GetDistance(const sPoint& P1, const sPoint& P2)
{
return sqrt((P1.x - P2.x)*(P1.x - P2.x) + (P1.y - P2.y)*(P1.y - P2.y));
}
sGridPoint ImageToGrid(const sPoint& P, double CellSize)
{
return sGridPoint((int)(P.x / CellSize), (int)(P.y / CellSize));
}
struct sGrid
{
sGrid(int W, int H, double CellSize)
: m_W(W)
, m_H(H)
, m_CellSize(CellSize)
{
m_Grid.resize((m_H));
for (auto i = m_Grid.begin(); i != m_Grid.end(); i++){ i->resize(m_W); }
}
void Insert(const sPoint& P)
{
sGridPoint G = ImageToGrid(P, m_CellSize);
m_Grid[G.x][G.y] = P;
}
bool IsInNeighbourhood(sPoint Point, double MinDist, double CellSize)
{
sGridPoint G = ImageToGrid(Point, CellSize);
//number of adjacent cell to look for neighbour points
const int D = 5;
// Scan the neighbourhood of the Point in the grid
for (int i = G.x - D; i < G.x + D; i++)
{
for (int j = G.y - D; j < G.y + D; j++)
{
if (i >= 0 && i < m_W && j >= 0 && j < m_H)
{
sPoint P = m_Grid[i][j];
if (P.m_valid && GetDistance(P, Point) < MinDist){ return true; }
}
}
}
return false;
}
private:
int m_H;
int m_W;
double m_CellSize;
std::vector< std::vector< sPoint> > m_Grid;
};
template <typename PRNG>
sPoint PopRandom(std::vector<sPoint>& Points, PRNG& Generator)
{
const int Idx = Generator.RandomInt(Points.size() - 1);
const sPoint P = Points[Idx];
Points.erase(Points.begin() + Idx);
return P;
}
template <typename PRNG>
sPoint GenerateRandomPointAround(const sPoint& P, double MinDist, PRNG& Generator)
{
// Start with non-uniform distribution
double R1 = Generator.RandomDouble();
double R2 = Generator.RandomDouble();
// radius should be between MinDist and 2 * MinDist
double Radius = MinDist * (R1 + 1.0f);
//random angle
double Angle = 2 * 3.141592653589f * R2;
// the new point is generated around the point (x, y)
double X = P.x + Radius * cos(Angle);
double Y = P.y + Radius * sin(Angle);
return sPoint(X, Y);
}
// Return a vector of generated points
// NewPointsCount - refer to bridson-siggraph07-poissondisk.pdf
// for details (the value 'k')
// Circle - 'true' to fill a circle, 'false' to fill a rectangle
// MinDist - minimal distance estimator, use negative value for default
template <typename PRNG = DefaultPRNG>
std::vector<sPoint> GeneratePoissonPoints(rsize_t NumPoints, PRNG& Generator, int NewPointsCount = 30,
bool Circle = true, double MinDist = -1.0f)
{
if (MinDist < 0.0f)
{
MinDist = sqrt(double(NumPoints)) / double(NumPoints);
}
std::vector <sPoint> SamplePoints;
std::vector <sPoint> ProcessList;
// create the grid
double CellSize = MinDist / sqrt(2.0f);
int GridW = (int)(ceil)(1.0f / CellSize);
int GridH = (int)(ceil)(1.0f / CellSize);
sGrid Grid(GridW, GridH, CellSize);
sPoint FirstPoint;
do
{
FirstPoint = sPoint(Generator.RandomDouble(), Generator.RandomDouble());
} while (!(Circle ? FirstPoint.IsInCircle() : FirstPoint.IsInRectangle()));
//Update containers
ProcessList.push_back(FirstPoint);
SamplePoints.push_back(FirstPoint);
Grid.Insert(FirstPoint);
// generate new points for each point in the queue
while (!ProcessList.empty() && SamplePoints.size() < NumPoints)
{
#if POISSON_PROGRESS_INDICATOR
// a progress indicator, kind of
if (SamplePoints.size() % 100 == 0) std::cout << ".";
#endif // POISSON_PROGRESS_INDICATOR
sPoint Point = PopRandom<PRNG>(ProcessList, Generator);
for (int i = 0; i < NewPointsCount; i++)
{
sPoint NewPoint = GenerateRandomPointAround(Point, MinDist, Generator);
bool Fits = Circle ? NewPoint.IsInCircle() : NewPoint.IsInRectangle();
if (Fits && !Grid.IsInNeighbourhood(NewPoint, MinDist, CellSize))
{
ProcessList.push_back(NewPoint);
SamplePoints.push_back(NewPoint);
Grid.Insert(NewPoint);
continue;
}
}
}
#if POISSON_PROGRESS_INDICATOR
std::cout << std::endl << std::endl;
#endif // POISSON_PROGRESS_INDICATOR
return SamplePoints;
}
}
and the main program is:
poisson.cpp
#include "stdafx.h"
#include <vector>
#include <iostream>
#include <fstream>
#include <memory.h>
#define POISSON_PROGRESS_INDICATOR 1
#include "PoissonGenerator.h"
const int NumPoints = 20000; // minimal number of points to generate
int main()
{
PoissonGenerator::DefaultPRNG PRNG;
const auto Points =
PoissonGenerator::GeneratePoissonPoints(NumPoints,PRNG);
std::ofstream File("Poisson.txt", std::ios::out);
File << "NumPoints = " << Points.size() << std::endl;
for (const auto& p : Points)
{
File << " " << p.x << " " << p.y << std::endl;
}
system("PAUSE");
return 0;
}
Suppose you have a point in the space [0,1] x [0,1], in the form of a std::pair<double, double>, but desire points in the space [x,y] x [w,z].
The function object
struct ProjectTo {
double x, y, w, z;
std::pair<double, double> operator(std::pair<double, double> in)
{
return std::make_pair(in.first * (y - x) + x, in.second * (z - w) + w);
}
};
will transform such an input point into the desired output point.
Suppose further you have a std::vector<std::pair<double, double>> points, all drawn from the input distribution.
std::copy(points.begin(), points.end(), points.begin(), ProjectTo{ x, y, w, z });
Now you have a vector of points in the output space.
I am having a problem with changing the coordinate of the point to ( 7,4) using the pointer variable. I just did x = 7 and y = 4, but I don't think that is correct. Can someone help ?
What I need to do:
in main()
instantiate a Point object and initialize at the time of definition
define a pointer that points to the object defined above
using the pointer variable to
update the coordinates of the point to (7,4)
display the distance from the origin
#include <iostream>
#include <math.h>
using namespace std;
class Point
{
private:
int x, y;
public:
Point(int x_coordinate, int y_coordinate);
int getVal();
double distance(double x2, double y2);
};
// Initialize the data members
Point::Point(int x_coordinate, int y_coordinate)
{
x = x_coordinate;
y = y_coordinate;
}
// Get the values of the data members.
int Point::getVal()
{
return x,y;
}
// Calculates and returns the point's distance from the origin.
double Point::distance(double x2, double y2)
{
double d;
d = sqrt( ((x2 - 0)*(x2 - 0)) + ((y2 - 0) * (y2 - 0)) );
return d;
}
//Allows user input and changes the point to (7,4) and displays the distance from origin.
int main()
{
int x,y;
cout << "Enter x coordinate followed by the y coordinate: " << endl;
cin >> x >> y;
Point p(x,y);
Point *newPointer = &p;
double theDistance = p.distance(x,y);
cout << "The point's distance from the origin is: " << theDistance << endl;
system("PAUSE");
}
To update coordinates of point, you need a new function -
void Point::UpdateCoordinates(int x0, int y0)
{
x = x0;
y = y0;
}
For distance(), I think you only need below.
double Point::distance()
{
return sqrt( x*x + y*y );
}
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.