# Spring phisic and quaternion

This topic is 1267 days old which is more than the 365 day threshold we allow for new replies. Please post a new topic.

## Recommended Posts

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?

##### Share on other sites
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. Edited by Álvaro

##### Share on other sites

Yes, you understood correctly.

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

##### Share on other sites

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


Edited by Álvaro

##### Share on other sites

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.

##### Share on other sites

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...

Edited by Waterlimon

##### Share on other sites
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.

##### Share on other sites

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.

##### Share on other sites

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