public libWrist.TransformMatrix getEditedTransform()
{
double[] centerRotation = new double[3];
double[] rotationAngles = new double[3];
double[] translation = new double[3];
centerRotation[0] = (double)numericUpDownCenterX.Value;
centerRotation[1] = (double)numericUpDownCenterY.Value;
centerRotation[2] = (double)numericUpDownCenterZ.Value;
rotationAngles[0] = (double)numericUpDownRotX.Value;
rotationAngles[1] = (double)numericUpDownRotY.Value;
rotationAngles[2] = (double)numericUpDownRotZ.Value;
translation[0] = (double)numericUpDownTransX.Value;
translation[1] = (double)numericUpDownTransY.Value;
translation[2] = (double)numericUpDownTransZ.Value;
libWrist.TransformMatrix rot = new libWrist.TransformMatrix();
libWrist.TransformMatrix t = new libWrist.TransformMatrix();
rot.rotateAboutCenter(rotationAngles, centerRotation);
t.setTranslation(translation);
return(t * rot);
}
/// <summary>
/// ////////////////////
/// </summary>
/// <returns></returns>
public libWrist.TransformMatrix setNewEditedTransform(float x, float y, float z, float q0, float q1, float q2, float q3)
{
double[] centerRotation = new double[3];
double[] rotationAngles = new double[3];
double[] translation = new double[3];
centerRotation[0] = (double)numericUpDownCenterX.Value;
centerRotation[1] = (double)numericUpDownCenterY.Value;
centerRotation[2] = (double)numericUpDownCenterZ.Value;
//use a method specially designed for getting roll, pitch, and yaw
float[] quat = new float[4];
quat[0] = q0;
quat[1] = q1;
quat[2] = q2;
quat[3] = q3;
float[] euler = new float[3];
quat2euler(quat, euler);
double roll = euler[2];
double pitch = euler[1];
double yaw = euler[0];
//keep small number
if (roll > 3.1416)
{
roll = -Math.PI + (roll - Math.PI);
}
if (roll < -3.1416)
{
roll = Math.PI + (roll - (-1) * Math.PI);
}
if (pitch > 3.1416)
{
pitch = -Math.PI + (pitch - Math.PI);
}
if (pitch < -3.1416)
{
pitch = Math.PI + (pitch - (-1) * 3.1416F);
}
if (yaw > 3.1416)
{
yaw = -Math.PI + (yaw - Math.PI);
}
if (yaw < -3.1416)
{
yaw = Math.PI + (yaw - (-1) * Math.PI);
}
//test
numericUpDownRotX.Value = (decimal)yaw;
numericUpDownRotY.Value = (decimal)pitch;
numericUpDownRotZ.Value = (decimal)roll;
rotationAngles[0] = (double)numericUpDownRotX.Value;
rotationAngles[1] = (double)numericUpDownRotY.Value;
rotationAngles[2] = (double)numericUpDownRotZ.Value;
x = (float)numericUpDownTransX.Value + x;
y = (float)numericUpDownTransY.Value + y;
z = (float)numericUpDownTransZ.Value + z;
if (x > 350)
{
x = 350;
}
if (x < -350)
{
x = -350;
}
if (y > 350)
{
y = 350;
}
if (y < -350)
{
y = -350;
}
if (z > 350)
{
z = 350;
}
if (z < -350)
{
z = -350;
}
numericUpDownTransX.Value = (decimal)x;
numericUpDownTransY.Value = (decimal)y;
numericUpDownTransZ.Value = (decimal)z;
translation[0] = (double)numericUpDownTransX.Value;
translation[1] = (double)numericUpDownTransY.Value;
translation[2] = (double)numericUpDownTransZ.Value;
libWrist.TransformMatrix rot = new libWrist.TransformMatrix();
libWrist.TransformMatrix t = new libWrist.TransformMatrix();
rot.rotateAboutCenter(rotationAngles, centerRotation);
//rotations go in order Z Y X
t.setTranslation(translation);
return(t * rot);
}