/// <summary> initail car Infomation </summary>
public void Init()
{
tPosition.Init();
tMotorPosition.Init();
tCarTirepositionL.Init();
tCarTirepositionR.Init();
eAngle = 0;
eCarTireSpeedLeft = 0;
eCarTireSpeedRight = 0;
eWheelAngle = 0;
}
/// <summary> 求角度差距: a_tV_Src轉到a_tV_Target 是順時針角度為正 否則為負 </summary>
public static double GetTheta_Difference(rtVector a_tV_Src, rtVector a_tV_Target)
{
double eTheta = 0, eCross = 0;
rtVector tCenter = new rtVector();
tCenter.Init();
eTheta = GetTheta(a_tV_Src, a_tV_Target);
eCross = Cross(a_tV_Src, a_tV_Target);
if (eCross < 0)
{
return(-eTheta);
}
else
{
return(eTheta);
}
}
/// <summary> 求角度差距: a_tV_Src轉到a_tV_Target 是順時針角度為正 否則為負 </summary>
public static double GetTheta_Difference(rtVector a_tV_Src, rtVector a_tV_Target)
{
double eTheta = 0, eCross = 0;
rtVector tCenter = new rtVector();
tCenter.Init();
eTheta = GetTheta(a_tV_Src, a_tV_Target);
eCross = Cross(a_tV_Src, a_tV_Target);
if (eCross < 0)
{
return -eTheta;
}
else
{
return eTheta;
}
}
/// <summary> Determine Path Vector in Turn Mode </summary>
/// <param name="a_tPosition">[IN] car position </param>
/// <param name="a_tTurnCenter">[IN] Turn Center </param>
/// <param name="a_lTurnDirection">[IN] Turn Direction </param>
/// <returns> Path Vector </returns>
public rtVector PathVectorDetermine_TurnMode(rtVector a_tPosition, rtVector a_tTurnCenter, int a_lTurnDirection)
{
rtVector tPathVector = new rtVector();
rtVector tVcenter2current = new rtVector();
tVcenter2current = rtVectorOP_2D.GetVector(a_tTurnCenter, a_tPosition);
switch (a_lTurnDirection)
{
case (int)rtTurnType_Simple.CAR_TURN_RIGHT: // 取右側的法向量 為路徑切線向量
tPathVector.eX = tVcenter2current.eY;
tPathVector.eY = -tVcenter2current.eX;
break;
case (int)rtTurnType_Simple.CAR_TURN_LEFT: // 取左側的法向量 為路徑切線向量
tPathVector.eX = -tVcenter2current.eY;
tPathVector.eY = tVcenter2current.eX;
break;
default: // 錯誤
tPathVector.Init();
break;
}
return tPathVector;
}