四元数实现

Vector3D.h

#pragma once
#include <iostream>

class Vector3D {
public:
    double x, y, z;

    Vector3D() : x(0), y(0), z(0) {}
    Vector3D(double x, double y, double z) : x(x), y(y), z(z) {}

    Vector3D operator+(const Vector3D& other) const;
    Vector3D operator-(const Vector3D& other) const;
    Vector3D operator-() const;
    double operator*(const Vector3D& other) const; // Dot product
    Vector3D operator^(const Vector3D& other) const; // Cross product

    Vector3D operator*(double scalar) const;
    Vector3D operator/(double scalar) const;
    friend Vector3D operator*(double scalar, const Vector3D& vec);

    Vector3D& operator+=(const Vector3D& other);
    Vector3D& operator-=(const Vector3D& other);
    bool operator==(const Vector3D& other) const;
    bool operator!=(const Vector3D& other) const;

    double magnitude() const;
    Vector3D normalize() const;
    double squaredNorm() const;

    void print() const;
};

Vector3D.cpp

#include "Vector3D.h"

Vector3D Vector3D::operator+(const Vector3D& other) const {
    return Vector3D(x + other.x, y + other.y, z + other.z);
}

Vector3D Vector3D::operator-(const Vector3D& other) const {
    return Vector3D(x - other.x, y - other.y, z - other.z);
}

Vector3D Vector3D::operator-() const {
    return Vector3D(-x, -y, -z);
}

double Vector3D::operator*(const Vector3D& other) const {
    return x * other.x + y * other.y + z * other.z;
}

Vector3D Vector3D::operator^(const Vector3D& other) const {
    return Vector3D(
        y * other.z - z * other.y,
        z * other.x - x * other.z,
        x * other.y - y * other.x
    );
}

Vector3D& Vector3D::operator+=(const Vector3D& other) {
    x += other.x;
    y += other.y;
    z += other.z;
    return *this;
}

Vector3D& Vector3D::operator-=(const Vector3D& other) {
    x -= other.x;
    y -= other.y;
    z -= other.z;
    return *this;
}

Vector3D Vector3D::operator*(double scalar) const {
    return Vector3D(x * scalar, y * scalar, z * scalar);
}
Vector3D Vector3D::operator/(double scalar) const {
    return Vector3D(x / scalar, y / scalar, z / scalar);
}

Vector3D operator*(double scalar, const Vector3D& vec) {
    return Vector3D(vec.x * scalar, vec.y * scalar, vec.z * scalar);
}

bool Vector3D::operator==(const Vector3D& other) const
{
    return other.x == x && other.y == y && other.z == z;
}

bool Vector3D::operator!=(const Vector3D& other) const
{
    return other.x != x || other.y != y || other.z != z;
}

void Vector3D::print() const {
    std::cout << "(" << x << ", " << y << ", " << z << ")" << std::endl;
}

double Vector3D::magnitude() const
{
    return std::sqrt(squaredNorm());
}

Vector3D Vector3D::normalize() const
{
    double mag = magnitude();
    if (mag != 0.0)
    {
        return *this / mag;
    }
    return *this;
}

double Vector3D::squaredNorm() const
{
    return x * x + y * y + z * z;
}

Quaternion.h

#pragma once
#include "Vector3D.h"
#include <iostream>

class Quaternion {
public:
    double a;
    Vector3D v;

    Quaternion() : a(0), v() {}
    Quaternion(double a, double x, double y, double z) : a(a), v(x, y, z) {}
    Quaternion(double a, const Vector3D& v) : a(a), v(v) {}
    Quaternion(const Vector3D& v) : a(0), v(v) {}
    Quaternion(const Vector3D& axis, double angle); // Axis-angle constructor

    Quaternion operator+(const Quaternion& other) const;
    Quaternion operator-(const Quaternion& other) const;
    Quaternion operator*(const Quaternion& other) const;
    Quaternion operator/(const Quaternion& other) const;

    Quaternion operator/(double scalar) const;
    Quaternion operator*(double scalar) const;
    friend Quaternion operator*(double scalar, const Quaternion& q);

    bool operator==(const Quaternion& other) const;
    bool operator!=(const Quaternion& other) const;

    Quaternion conjugate();
    double magnitude();
    void print() const;
};

Quaternion.cpp

#include "Quaternion.h"
#include <cmath>

Quaternion::Quaternion(const Vector3D& axis, double angle) {
    a = cos(angle / 2.0);
    v = axis.normalize() * sin(angle / 2.0);
}

Quaternion Quaternion::operator+(const Quaternion& other) const {
    return Quaternion(a + other.a, v + other.v);
}

Quaternion Quaternion::operator-(const Quaternion& other) const {
    return Quaternion(a - other.a, v - other.v);
}

Quaternion Quaternion::operator*(const Quaternion& other) const {
    return Quaternion(a * other.a - v * other.v, (other.v * a) + (v * other.a) + (v ^ other.v));
}

Quaternion Quaternion::operator/(const Quaternion& other) const {
    return Quaternion(a * other.a - v * -other.v, (-other.v * a) + (v * other.a) - (v ^ other.v));
}

Quaternion Quaternion::operator/(double scalar) const {
    return Quaternion(a / scalar, v / scalar);
}

Quaternion Quaternion::operator*(double scalar) const {
    return Quaternion(a * scalar, v * scalar);
}

Quaternion operator*(double scalar, const Quaternion& q) {
    return Quaternion(q.a * scalar, q.v * scalar);
}

bool Quaternion::operator==(const Quaternion& other) const
{
    return a == other.a && v == other.v;
}

bool Quaternion::operator!=(const Quaternion& other) const
{
    return a != other.a || v != other.v;
}

void Quaternion::print() const {
    std::cout << "(" << a << ", " << v.x << ", " << v.y << ", " << v.z << ")" << std::endl;
}

Quaternion Quaternion::conjugate()
{
    return Quaternion(a, -v);
}

double Quaternion::magnitude()
{
    return std::sqrt(a * a + v.squaredNorm());
}

main.cpp

#include "Quaternion.h"
#include "Vector3D.h"
#include <iostream>
#define M_PI 3.1415926535
int main() {
    // Test Vector3D
    Vector3D v1(1, 1, 1);
    std::cout << "v1: ";
    v1.print();
    std::cout << v1.magnitude() << std::endl;

    Vector3D axis(0, 0, 1);
    double angle = M_PI / 4;
    Quaternion q(axis, angle);
    std::cout << "Quaternion q: ";
    q.print();
    std::cout << q.magnitude() << std::endl;

    Quaternion rotated = q * Quaternion(v1) / q;
    std::cout << "Rotated v1: ";
    rotated.print();
    std::cout << rotated.magnitude() << std::endl;

    return 0;
}