Dampening is next step. It would be more easier. Now it would be good if spring make harmonic vibration from start to target, opposite side and back, without unexpected rotation and flying into deep space.

Is that not what you are getting?

I added a `main' and a couple of extra operators for vectors to show that integrating this torque works. You can set `angular_dampening' to 0.0 if you want to see it oscillate forever. For simplicity, I am assuming the moment of inertia is the identity matrix.

#include <iostream>
#include <cmath>
#include <boost/math/quaternion.hpp>
typedef boost::math::quaternion<double> Quaternion;
struct Vector {
double x, y, z;
Vector(double x, double y, double z) : x(x), y(y), z(z) {
}
};
std::ostream &operator<<(std::ostream &os, Vector v) {
return os << '(' << v.x << ',' << v.y << ',' << v.z << ')';
}
Vector operator*(double s, Vector v) {
return Vector(s * v.x, s * v.y, s * v.z);
}
Vector operator-(Vector v1, Vector v2) {
return Vector(v1.x - v2.x, v1.y - v2.y, v1.z - v2.z);
}
Vector &operator+=(Vector &v1, Vector v2) {
v1.x += v2.x;
v1.y += v2.y;
v1.z += v2.z;
return v1;
}
Vector cross_product(Vector v1, Vector v2) {
return Vector(v1.y * v2.z - v1.z * v2.y,
v1.z * v2.x - v1.x * v2.z,
v1.x * v2.y - v1.y * v2.x);
}
double dot_product(Vector v1, Vector v2) {
return v1.x * v2.x + v1.y * v2.y + v1.z * v2.z;
}
double squared_length(Vector v) {
return dot_product(v, v);
}
double length(Vector v) {
return std::sqrt(squared_length(v));
}
Quaternion most_natural_rotation(Vector v1, Vector v2) {
Vector v = cross_product(v1, v2);
Quaternion q(std::sqrt(squared_length(v1) * squared_length(v2)) + dot_product(v1, v2),
v.x, v.y, v.z);
return q / abs(q);
}
Vector apply_rotation(Quaternion q, Vector v) {
Quaternion result = q * Quaternion(0.0, v.x, v.y, v.z) * conj(q);
return Vector(result.R_component_2(), result.R_component_3(), result.R_component_4());
}
Vector log_of_unit_quaternion(Quaternion q) {
double angle = std::acos(q.real());
Vector axis(q.R_component_2(), q.R_component_3(), q.R_component_4());
return (angle / length(axis)) * axis;
}
Vector compute_torque(Quaternion current_attitude,
Vector model_needle,
Vector target,
double spring_force) {
Vector needle = apply_rotation(current_attitude, model_needle);
Quaternion rotation = most_natural_rotation(needle, target);
return spring_force * log_of_unit_quaternion(rotation);
}
int main() {
Vector model_needle(1.0, 0.0, 0.0);
Quaternion attitude(1.0, 0.0, 0.0, 0.0);
Vector angular_velocity(0.0, 0.0, 0.0);
double const angular_dampening = 1.0;
double const dt = 0.1;
for (int i=0; i<1000; ++i) {
std::cout << apply_rotation(attitude, model_needle) << ' ' << attitude << ' ' << angular_velocity << '\n';
Vector torque = compute_torque(attitude, model_needle, Vector(0.0, 1.0, 0.0), 1.0);
Vector angular_acceleration = torque - angular_dampening * angular_velocity;
angular_velocity += dt * angular_acceleration;
attitude *= exp(dt * Quaternion(0.0, angular_velocity.x, angular_velocity.y, angular_velocity.z));
}
}