//FOR ALL SEGMENTS
public static void Gen_QL(Segment S_self, Segment S_parent)
{
Segment Sc = S_self;
Segment Sp = S_parent;
if (Sc.ID == Sp.ID)//ID=11
{
Sc.QL = Sc.QW;
}
else
{
CQuaternion tmp = new CQuaternion(Sp.QW.m_q0, Sp.QW.m_q1, Sp.QW.m_q2, Sp.QW.m_q3);
tmp.Conjugate();
Sc.QL = tmp * Sc.QW;
}
if (Sc.isFirstTime)
{
Sc.PrevRot = new Euler();
Sc.isFirstTime = false;
}
else
{
Sc.PrevRot = new Euler(Sc.Eul_Out.Eul_1, Sc.Eul_Out.Eul_2, Sc.Eul_Out.Eul_3);
}
Euler ang = CQuaternion.Quat2angle(Sc.QL.m_q0, Sc.QL.m_q1, Sc.QL.m_q2, Sc.QL.m_q3, 3);
double y = ang.Eul_1;
double x = ang.Eul_2;
double z = ang.Eul_3;
Sc.Eul_Out.Eul_1 = y * 180 / System.Math.PI;
Sc.Eul_Out.Eul_2 = x * 180 / System.Math.PI;
Sc.Eul_Out.Eul_3 = z * 180 / System.Math.PI;
#region DeQUIVER
Euler diff = new Euler((Sc.PrevRot.Eul_1 - Sc.Eul_Out.Eul_1), (Sc.PrevRot.Eul_2 - Sc.Eul_Out.Eul_2), (Sc.PrevRot.Eul_3 - Sc.Eul_Out.Eul_3));
//DEBUG
#region DEBUG FOR DE-QUIVER
/*if (Sc.ID == 11)
{
// Console.WriteLine(" Prev:" + Sc.PrevRot.ToString());
Console.WriteLine(" Diff:" + diff.ToString());
// Console.WriteLine(" Post:" + Sc.Eul_Out.ToString());
}*/
#endregion
#region De_GIMBOL_LOCK
if (System.Math.Abs(diff.Eul_1) < 100)
{
#region GIMBAL LOCK REMEDY PART2
//if (PrevRot.Eul_3 > 90)
//{
// PrevRot.Eul_3 = PrevRot.Eul_3 - 180;
// Sc.Eul_Out.Eul_3 = Sc.Eul_Out.Eul_3 - 180;
//}
//if (PrevRot.Eul_3 < -90)
//{
// PrevRot.Eul_3 = PrevRot.Eul_3 + 180;
// Sc.Eul_Out.Eul_3 = Sc.Eul_Out.Eul_3 + 180;
//}
#endregion
Sc.Eul_Out.Eul_1 = (3 * Sc.PrevRot.Eul_1 + Sc.Eul_Out.Eul_1) / 4;
}
else
{
Console.WriteLine("Y rot GimbolLock!");
#region GIMBAL LOCK REMEDY PART1
//if (PrevRot.Eul_1 > 0 && Sc.Eul_Out.Eul_1<0)
//{
// Console.WriteLine("Y rot GimbolLock --Prev: " + PrevRot.Eul_1.ToString()+ " --Post: " +Sc.Eul_Out.Eul_1);
// Sc.Eul_Out.Eul_1 = Sc.Eul_Out.Eul_1 + 360;
// Sc.Eul_Out.Eul_1 = (3 * PrevRot.Eul_1 + Sc.Eul_Out.Eul_1) / 4;
//}
//else if (PrevRot.Eul_1 < 0 && Sc.Eul_Out.Eul_1 > 0)
//{
// Console.WriteLine("Y rot GimbolLock --Prev: " + PrevRot.Eul_1.ToString() + " --Post: " + Sc.Eul_Out.Eul_1);
// Sc.Eul_Out.Eul_1 = Sc.Eul_Out.Eul_1 - 360;
// Sc.Eul_Out.Eul_1 = (3 * PrevRot.Eul_1 + Sc.Eul_Out.Eul_1) / 4;
//}
#endregion
}
if (System.Math.Abs(diff.Eul_2) < 100)
{
Sc.Eul_Out.Eul_2 = (3 * Sc.PrevRot.Eul_2 + Sc.Eul_Out.Eul_2) / 4;
}
else
{
Console.WriteLine("X rot GimbolLock!");
}
if (System.Math.Abs(diff.Eul_3) < 85)
{
Sc.Eul_Out.Eul_3 = (3 * Sc.PrevRot.Eul_3 + Sc.Eul_Out.Eul_3) / 4;
}
else
{
Console.WriteLine("Z rot GimbolLock!");
}
#endregion
#endregion
if (Sc.ID == Sp.ID)
{
Sc.Qlocal = CQuaternion.angle2Quat(y, x, z, 3);
Sc.Qworld = Sc.Qlocal;
}
else
{
Sc.Qlocal = CQuaternion.angle2Quat(y, x, z, 3);
Sc.Qworld = Sp.Qworld * Sc.Qlocal;
}
}
//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
}
//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();
}
