Hi

I am trying to make simulation of mechanical spring affecting on a body fixed by ball joint connection and forcing it to take vertical orientation with torque proportional to angles of deviation from global axes.

I use Open Dinamic Engine for simulation.

Orientation of a body defined with quaternion Q= (w,x,y,z). How can I found angles on wich body rotated to global axes?

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.

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.

#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?