//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;
}
}
public Segment(int self_id, int parent_id)
{
ID = self_id;
parentID = parent_id;
raw = new SensorData();
Q_init = new CQuaternion();
QW = new CQuaternion();
QL = new CQuaternion();
Eul_Out = new Euler();
PrevRot = new Euler();
Qworld = new CQuaternion();
Qlocal = new CQuaternion();
Position = new Pos();
isFirstTime = true;
}
