//FOR ARMS
public static void Calibration_horizontal(Segment S)
{
Global.PrevRot = new Euler(0, 0, 0);
CQuaternion tmp = new CQuaternion(S.raw.Q.m_q0, S.raw.Q.m_q1, S.raw.Q.m_q2, S.raw.Q.m_q3);
S.Q_init = tmp.Conjugate();
if (S.ID == 11)//S.ID == 10||
{
CQuaternion Zref = new CQuaternion(0, 0, 0, 1);
CQuaternion ZW0 = Zref * S.Q_init;
CQuaternion ZW = S.raw.Q * ZW0;
CQuaternion YW = new CQuaternion(0, ZW.m_q1, ZW.m_q2, 0);
double sqrtYW = 1/System.Math.Sqrt(ZW.m_q1*ZW.m_q1 + ZW.m_q2*ZW.m_q2);
CQuaternion YWnorm = new CQuaternion(0, YW.m_q1*sqrtYW, YW.m_q2*sqrtYW, 0);
V3 SS = new V3(YWnorm.m_q1*0.5, (YWnorm.m_q2+1)*0.5, YWnorm.m_q3*0.5);
double w = System.Math.Sqrt(SS.x*SS.x + SS.y*SS.y + SS.z*SS.z);
V3 normSS = new V3(SS.x/w, SS.y/w, SS.z/w);
V3 Yref = new V3(0, 1, 0);
V3 z = V3.cross(Yref, normSS);
Global.Qtc = new CQuaternion(w, z.x, z.y, z.z);
Global.invQtc = new CQuaternion(Global.Qtc.m_q0, -Global.Qtc.m_q1, -Global.Qtc.m_q2, -Global.Qtc.m_q3);
if (Global.DEBUG_TPose)
{
Console.WriteLine("Qtc = " + Global.Qtc.ToString());
Console.WriteLine("Calibration Done!");
}
}
if (S.ID == 14)
{
S.Position = new Pos(0.1125, 0.0, 0.27); // Left TOE POSITION
}
if (S.ID == 17)
{
S.Position = new Pos(-0.1125, 0.0, 0.27);// Right TOE POSITION
}
Global.HIP = new Pos();
}
//FOR ARMS
public static void Gen_QW_horizontal(Segment S_self)
{
Segment S1 = S_self;
CQuaternion q_current = S1.raw.Q;
CQuaternion tmp0 = q_current * S1.Q_init;
CQuaternion tmp1 = tmp0 * Global.Qtc;
CQuaternion tmp2 = Global.invQtc * tmp1;
S1.QW = new CQuaternion(tmp2.m_q0, -tmp2.m_q1, tmp2.m_q3, tmp2.m_q2);
#region DisableShoulderJoints
if (S1.ID == 4 || S1.ID == 5)
{
S1.QW = new CQuaternion(SegmentCollection.arraySegment[10].QW.m_q0, SegmentCollection.arraySegment[10].QW.m_q1, SegmentCollection.arraySegment[10].QW.m_q2, SegmentCollection.arraySegment[10].QW.m_q3);
}
#endregion
#region DEBUG_AZIMUTH
if (Global.DEBUG_AZIMUTH)
{
if (S1.ID == 11)
{
CQuaternion Zref = new CQuaternion(0, 0, 0, 1);
CQuaternion ZW0 = Zref * S1.Q_init;
CQuaternion ZW = S1.raw.Q * ZW0;
CQuaternion YW = new CQuaternion(0, ZW.m_q1, ZW.m_q2, 0);
double sqrtYW = 1 / System.Math.Sqrt(ZW.m_q1 * ZW.m_q1 + ZW.m_q2 * ZW.m_q2);
CQuaternion YWnorm = new CQuaternion(0, YW.m_q1 * sqrtYW, YW.m_q2 * sqrtYW, 0);
V3 SS = new V3(YWnorm.m_q1 * 0.5, (YWnorm.m_q2 + 1) * 0.5, YWnorm.m_q3 * 0.5);
double w = System.Math.Sqrt(SS.x * SS.x + SS.y * SS.y + SS.z * SS.z);
V3 normSS = new V3(SS.x / w, SS.y / w, SS.z / w);
V3 Yref = new V3(0, 1, 0);
V3 z = V3.cross(Yref, normSS);
//Global.Qtc = new CQuaternion(w, z.x, z.y, z.z);
Global.QAZIMUTH = new CQuaternion(w, z.x, z.y, z.z);
CQuaternion DeltaQ = Global.QAZIMUTH * Global.invQtc;
Euler EAZIMUTH = CQuaternion.Quat2angle(DeltaQ.m_q0, DeltaQ.m_q1, DeltaQ.m_q2, DeltaQ.m_q3, 1);
double Azimuth = EAZIMUTH.Eul_1 * 57.3;
if (Global.DEBUG_AZIMUTH)
Console.WriteLine("AZIMUTH = " + Global.QAZIMUTH.ToString());
}
}
#endregion
}
public static V3 cross(V3 I, V3 J)
{
double x = I.y * J.z - J.y * I.z;
double y = -1 * (I.x * J.z - J.x * I.z);
double z = I.x * J.y - J.x * I.y;
V3 result = new V3(x, y, z);
return result;
}
