Spring phisic and quaternion

I am not entirely sure I understand what you are trying to do, but there is a good chance that you don't actually want angles, even if you think you do.

So your body has something like a needle and you want to have some corrective force that tries to make the needle point upwards, right? You can compute a direction in the space of angular velocities that corresponds to the most natural rotation that would make the needle point upwards, and apply a torque proportional to it. Do you think that would satisfy your requirements? We can figure the exact formulas later.

Yes, you understood correctly.

I do not know how exactly realize what I want. I would assay all variants.

This should be close to what you need. It is a bit math heavy, so don't feel too bad if you don't understand it all: Just ask me about what parts you don't understand and I'll try to explain them.

#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 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);
}

Thank you.
It seems I understand what that code doing. But as I try to do the same as your example and use it in my application, the body instead of aligning begins random rotation. Can you check? May be I doing something wrong, or it not exactly that needed.

You probably also want to add a dampening term to the compute_torque function if im not wrong (-angular_velocity*dampening_force appended to the return statement I think, of course add the variables as params), otherwise it will easily keep oscillating forever.

edit: or not if you actually want conservation of energy in your model...

You probably do want dampening, but I think of dampening as something separate from the torque computation. In the analogous situation of simulating a pendulum, I wouldn't make my gravity term contain a dampening force. The code I wrote plays the role of computing the acceleration due to gravity. You also need friction.

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.

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));
  }
}

alvaro said:
log_of_unit_quaternion

I was wondering how did you choose this name for the function?

Why log? log as in logarithm doesn't make sense to me.

Or why log?

(Oh, I think you meant log as in logging information? Saving information?)

But, I don't understand this function.. It takes the rotation as argument, and scales the axis part of this rotation quaternion, right?

Why does this lead to values, we can use as a torque vector?

Yes, it's “log” for “logarithm”: https://en.wikipedia.org/wiki/Quaternion#Exponential,_logarithm,_and_power_functions

It's been too long since I wrote this code, so I don't have all the details in my head, but the logarithm of a quaternion gives you the angular velocity you would have to integrate for one unit of time to get to that rotation (or something very close to that; I might be off by a factor of 2), so it's a good way to measure how far off from not rotating you are, and in which direction.

Ok. thanks.

alvaro said:
logarithm of a quaternion gives you the angular velocity

Wow! log of quaternion is related to torque.. amazing. Yet again a thing I was completely unaware of. Thanks!

brebarth221 said:

Ok. thanks.

alvaro said:
logarithm of a quaternion gives you the angular velocity

Wow! log of quaternion is related to torque.. amazing. Yet again a thing I was completely unaware of. Thanks!

If it helps, this is much easier to understand in the parallel case of 2D rotation being represented by a mudulus-1 complex number. If you identify points on the plane with complex numbers, a rotation by an angle alpha can be computed by multiplying by z = cos(alpha)+i*sin(alpha) == exp(i*alpha).

exp(k) can be thought of as, where would I end up if I start at 1 and then for one unit of time I move with velocity that is computed by multiplying my current position by k. When k is i*alpha, you are moving in a direction perpendicular to your current position, which makes you rotate, and alpha determines how quickly you rotate.

If you want to recover the angle from the complex number, you can do it by using log(z), which gives you i*alpha. In 3D, you get coefficients for i, j and k, telling you in which direction to rotate.

Thanks.. It will take time to understand.. But I will try.

This is helpful for me to begin to understand there is a relationship between rotation and exp():

cos(alpha)+i*sin(alpha) == exp(i*alpha)