Jump to content
  • Advertisement
Sign in to follow this  
Trombon

IK constraints Quat-Euler, Euler-Quat

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

If you intended to correct an error in the post then please contact us.

Recommended Posts

Hello I wonder on how to proceed on this...I have these two quaternions, representing start/goal for an IK chain. I want it to rotate a selected joint so the effector gets as close to the goal as possible...considering its local constraints. I wish (if possible) to calculate a rotation quaternion, describing the rotation from start to goal. Then convert that rotation into Euler angles....then adjust these angles so it rotates as much as it can about its axis...ex limits: x(-5,5) y(10,40) z (-15,15) euler: (-10,30,20) it gets adjusted to (-5,30,15)...then converted back into a rotation quaternion....and used to calculate a "practical" goal...the closest position to the real goal it can reach. Im not sure if this is going to work...or if there is a better way. (interested to hear about it) and I could really use som conversion routines for the rotation Quaternion->Euler and Euler->Quaternion using XYZ order...and the coordinatesystem is as follows x is right y is up z is towards viewer any helpful answers, thoughts is most welcomed.... /T

Share this post


Link to post
Share on other sites
Advertisement
Guest Anonymous Poster

inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;
double roll = DegreesToRadians(x);
double pitch = DegreesToRadians(y);
double yaw = DegreesToRadians(z);

double cyaw, cpitch, croll, syaw, spitch, sroll;
double cyawcpitch, syawspitch, cyawspitch, syawcpitch;

cyaw = cos(0.5f * yaw);
cpitch = cos(0.5f * pitch);
croll = cos(0.5f * roll);
syaw = sin(0.5f * yaw);
spitch = sin(0.5f * pitch);
sroll = sin(0.5f * roll);

cyawcpitch = cyaw*cpitch;
syawspitch = syaw*spitch;
cyawspitch = cyaw*spitch;
syawcpitch = syaw*cpitch;

q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);

return q;
}



inline Vector MakeEulerAnglesFromQ(Quaternion q)
{
double r11, r21, r31, r32, r33, r12, r13;
double q00, q11, q22, q33;
double tmp;
Vector u;

q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;

r11 = q00 + q11 - q22 - q33;
r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
r33 = q00 - q11 - q22 + q33;

tmp = fabs(r31);

if(tmp > 0.999999)
{
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);

u.x = RadiansToDegrees(0.0f); //roll
u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
u.z = RadiansToDegrees((float) atan2(-r12,(-r31*r13))); // yaw

return u;
}

u.x = RadiansToDegrees((float) atan2(r32,r33)); // roll
u.y = RadiansToDegrees((float) asin(-r31)); // pitch
u.z = RadiansToDegrees((float) atan2(r21,r11)); // yaw
return u;


}

Share this post


Link to post
Share on other sites
<code>
inline Quaternion MakeQFromEulerAngles(float x, float y, float z)
{
Quaternion q;
double roll = DegreesToRadians(x);
double pitch = DegreesToRadians(y);
double yaw = DegreesToRadians(z);

double cyaw, cpitch, croll, syaw, spitch, sroll;
double cyawcpitch, syawspitch, cyawspitch, syawcpitch;

cyaw = cos(0.5f * yaw);
cpitch = cos(0.5f * pitch);
croll = cos(0.5f * roll);
syaw = sin(0.5f * yaw);
spitch = sin(0.5f * pitch);
sroll = sin(0.5f * roll);

cyawcpitch = cyaw*cpitch;
syawspitch = syaw*spitch;
cyawspitch = cyaw*spitch;
syawcpitch = syaw*cpitch;

q.n = (float) (cyawcpitch * croll + syawspitch * sroll);
q.v.x = (float) (cyawcpitch * sroll - syawspitch * croll);
q.v.y = (float) (cyawspitch * croll + syawcpitch * sroll);
q.v.z = (float) (syawcpitch * croll - cyawspitch * sroll);

return q;
}
</code>

<src>
inline Vector MakeEulerAnglesFromQ(Quaternion q)
{
double r11, r21, r31, r32, r33, r12, r13;
double q00, q11, q22, q33;
double tmp;
Vector u;

q00 = q.n * q.n;
q11 = q.v.x * q.v.x;
q22 = q.v.y * q.v.y;
q33 = q.v.z * q.v.z;

r11 = q00 + q11 - q22 - q33;
r21 = 2 * (q.v.x*q.v.y + q.n*q.v.z);
r31 = 2 * (q.v.x*q.v.z - q.n*q.v.y);
r32 = 2 * (q.v.y*q.v.z + q.n*q.v.x);
r33 = q00 - q11 - q22 + q33;

tmp = fabs(r31);

if(tmp > 0.999999)
{
r12 = 2 * (q.v.x*q.v.y - q.n*q.v.z);
r13 = 2 * (q.v.x*q.v.z + q.n*q.v.y);

u.x = RadiansToDegrees(0.0f); //roll
u.y = RadiansToDegrees((float) (-(pi/2) * r31/tmp)); // pitch
u.z = RadiansToDegrees((float) atan2(-r12,(-r31*r13))); // yaw

return u;
}

u.x = RadiansToDegrees((float) atan2(r32,r33)); // roll
u.y = RadiansToDegrees((float) asin(-r31)); // pitch
u.z = RadiansToDegrees((float) atan2(r21,r11)); // yaw
return u;


}
</src>


Well not sure how to do the source post, but anywho this should get ya started, make sure your roll, pitch, and yaw are on the same coordinates... been awhile since I went over this code....

Share this post


Link to post
Share on other sites
Sign in to follow this  

  • Advertisement
×

Important Information

By using GameDev.net, you agree to our community Guidelines, Terms of Use, and Privacy Policy.

Participate in the game development conversation and more when you create an account on GameDev.net!

Sign me up!