Advertisement
Not a member of Pastebin yet?
Sign Up,
it unlocks many cool features!
- #include <string>
- #include <iostream>
- 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(getY() + 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;
- }
- };
- 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"}"; }
- };
- 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"}"; }
- };
- int main() {
- 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();
- int a;
- std::cin >> a;
- }
Advertisement
Add Comment
Please, Sign In to add comment
Advertisement