public object Process()
{
if (!SetupConnection())
{
OnProcessError(ErrorCode.SetupConnection, "Setup connection error");
return(0);
}
int motionCount = GetMotionCount();
if (motionCount == -1)
{
OnProcessError(ErrorCode.MotionCount, "Motion count must be greater than -1");
return(0);
}
if (motionCount == -2)
{
return(0);
}
var listMotionInfo = new List <MotionInfo>();
for (int i = 0; i < motionCount; i++)
{
MotionInfo info = GetMotionInfoAtIndex(i);
if (info == null)
{
OnProcessError(ErrorCode.MotionInfo, "Motion info is null");
return(0);
}
listMotionInfo.Add(GetMotionInfoAtIndex(i));
}
OnProcessSuccessfully(listMotionInfo);
//Connection.ReleaseConnection();
return(1);
}
internal ParticleDefinition(Emitter owner, string name, Emitter.Frame[] frames)
{
Owner = owner;
Name = name;
this.frames = frames;
Alpha = new Behavior <float>()
{
From = 1, To = 1
};
Color = new Behavior <Color>()
{
From = XNA.Color.White, To = XNA.Color.White
};
Angle = new Behavior <float>()
{
From = 0, To = 0
};
Scale = new Behavior <float>()
{
From = 1, To = 1
};
Lifetime = new LifetimeInfo();
Motion = new MotionInfo();
OnComplete = delegate { };
Attractors = new List <ParticleAttractor>();
}
protected override void OnInit()
{
base.OnInit();
m_Collider = tankPlayer.GetComponent <BoxCollider>();
ChangeState(AIState.FindPowerup);
m_PlayerMotionInfo = new MotionInfo(MotionType.NavMeshAgent, tankPlayer.Position, tankPlayer.Velocity, tankPlayer.gameObject);
m_PredictionStep = Mathf.RoundToInt(m_PredictionTime / Time.fixedDeltaTime);
m_PathBuffer = new NavMeshPath();
}
/// <summary>
/// Inicializace motoru
/// </summary>
/// <param name="connector">connector sběrnice</param>
/// <param name="stateObserver">posluchač stavu motoru</param>
/// <param name="nodeNumber">číslo node</param>
/// <param name="id">id motoru</param>
/// <param name="mode">defaultní nastavení módu</param>
/// <param name="reverse">příznak obrácení směru točení</param>
/// <param name="multiplier">násobitel otáček v případě, že je motor za převodovkou</param>
/// <param name="positionVeocity">rychlost motoru v otáčkách při pozicování</param>
/// <param name="positionAceleration">zrychlení motoru v otáčkách při pozicování</param>
/// <param name="positionDeceleration">zpomalení motoru v otáčkách při pozicování</param>
/// <param name="velocity">maximální rychlost motoru při rychlostním řízení</param>
/// <param name="aceleration">zrychlení motoru při rychlostním řízení</param>
/// <param name="deceleration">zpomalení motoru při rychlostním řízení</param>
public void inicialize(DeviceManager connector, IStateObserver stateObserver, Action motorErrorOccuredObserver, int nodeNumber, MotorId id, MotorMode mode, bool reverse, int multiplier, uint positionVelocity, uint positionAceleration, uint positionDeceleration, uint velocity, uint aceleration, uint deceleration)
{
try
{
this.mode = mode;
this.stateObserver = stateObserver;
this.motorErrorOccuredObserver = motorErrorOccuredObserver;
this.id = id;
this.multiplier = multiplier;
if (reverse)
{
rev = -1;
}
motor = connector.CreateDevice(Convert.ToUInt16(nodeNumber));
stateHandler = motor.Operation.MotionInfo;
sm = motor.Operation.StateMachine;
if (sm.GetFaultState())
{
sm.ClearFault();
}
sm.SetEnableState();
maxSpeed = (int)velocity;
velocityHandler = motor.Operation.ProfileVelocityMode;
velocityHandler.SetVelocityProfile(aceleration, deceleration);
positionHandler = motor.Operation.ProfilePositionMode;
positionHandler.SetPositionProfile(positionVelocity, positionAceleration, positionDeceleration);
homingHandler = motor.Operation.HomingMode;
changeMode(mode);
setStateObserver();
state = MotorState.enabled;
stateObserver.motorStateChanged(MotorState.enabled, "", id, 0, 0, 0, 0);
targetPosition = stateHandler.GetPositionIs();
targetAngle = getAngleFromPosition(targetPosition);
}
catch (DeviceException e)
{
sm = null;
disableStateObserver();
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
}
}
//given two objects' motion infos and their sizes, predict if they will collide
//returns the frame number in which the collision occurs
//returns -1 if the collision will not happen within time step
private static int WillCollide(MotionInfo motionInfo1, MotionInfo motionInfo2, int timeStep)
{
//Debug.DrawLine(motionInfo1.bounds.center, motionInfo1.PredictPosition(timeStep).center, Color.blue);
//Debug.DrawLine(motionInfo2.bounds.center, motionInfo2.PredictPosition(timeStep).center, Color.red);
//simulate
for (int i = 1; i <= timeStep; i++)
{
if (motionInfo1.PredictPosition(i).Intersects(motionInfo2.PredictPosition(i)))
{
return(i);
}
}
//Debug.Log("will not collide");
return(-1);
}
private void SaveMotion()
{
JsonData jsonTemp1 = new JsonData();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < paraCount; j++)
{
MotionInfo motionInfo = new MotionInfo();
motionInfo.motionIndex = i;
motionInfo.boneIndex = j;
motionInfo.value = player1Para[i][j];
jsonTemp1.MotionList.Add(motionInfo);
}
}
string result1 = JsonUtility.ToJson(jsonTemp1, true);
StreamWriter sw1 = new StreamWriter(JsonPath(0), false);
sw1.Write(result1);
Debug.Log(result1);
sw1.Flush();
sw1.Close();
JsonData jsonTemp2 = new JsonData();
for (int i = 0; i < 2; i++)
{
for (int j = 0; j < paraCount; j++)
{
MotionInfo motionInfo = new MotionInfo();
motionInfo.motionIndex = i;
motionInfo.boneIndex = j;
motionInfo.value = player2Para[i][j];
jsonTemp2.MotionList.Add(motionInfo);
}
}
string result2 = JsonUtility.ToJson(jsonTemp2, true);
StreamWriter sw2 = new StreamWriter(JsonPath(1), false);
sw2.Write(result2);
Debug.Log(result2);
sw2.Flush();
sw2.Close();
}
private void PlayAnimation(MotionInfo clip, int hash, Action callback)
{
if (clip.motion != null)
{
if (_animationCoroutine != null)
{
StopCoroutine(_animationCoroutine);
}
_animationCoroutine = _PlayAnimationAndDisableAnimator(clip.motion.averageDuration + 0.1f, hash, callback);
StartCoroutine(_animationCoroutine);
}
else
{
animator.enabled = false;
if (callback != null)
{
callback();
}
}
}
public void MotionDataCopy(MotionInfo current_data)
{
index = current_data.index;
DisplayStart = current_data.DisplayStart;
VirtualStart = current_data.VirtualStart;
DisplayTarget = current_data.DisplayTarget;
VirtualTarget = current_data.VirtualTarget;
VirtualTarget2 = current_data.VirtualTarget2;
Direction = current_data.Direction;
Direction2 = current_data.Direction2;
Velocity = current_data.Velocity;
Rotate_Speed = current_data.Rotate_Speed;
SpindleSpeed = current_data.SpindleSpeed;
Time_Value = current_data.Time_Value;
Time_Value2 = current_data.Time_Value2;
Center_Point = current_data.Center_Point;
Rotate_Degree = current_data.Rotate_Degree;
Motion_Type = current_data.Motion_Type;
Current_Plane = current_data.Current_Plane;
Immediate_Motion = current_data.Immediate_Motion;
ToolChange_Motion = current_data.ToolChange_Motion;
Slash = current_data.Slash;
Tool_Number = current_data.Tool_Number;
D_Value = current_data.D_Value;
H_Value = current_data.H_Value;
ListCopy_MotionData(current_data.G_Display, current_data.G_Display2, current_data.G_Address, current_data.G_Address2, current_data.ModalString, current_data.Address_Value, current_data.Address_Value2, current_data.ModalIndex);
Remaining_Movement = new float[3] {
current_data.Remaining_Movement[0], current_data.Remaining_Movement[1], current_data.Remaining_Movement[2]
};
CooTransformation = current_data.CooTransformation;
CooState = new bool[] { current_data.CooState[0], current_data.CooState[1], current_data.CooState[2] };
M_Code = current_data.M_Code;
RadiusCompensationInfo = current_data.RadiusCompensationInfo;
LengthCompensationInfo = current_data.LengthCompensationInfo;
RadiusState = current_data.RadiusState;
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2) //直线圆弧
{
float k1;
float b1;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R = Vector3.Distance(motion_data2.Center_Point, motion_data2.DisplayStart);
if (motion_data1.DisplayTarget.x == motion_data1.DisplayStart.x) //K1不存在
{
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
float a = (R - Radius2) * (R - Radius2);
float b = Mathf.Pow(motion_data2.DisplayStart.x - motion_data2.Center_Point.x, 2);
if ((a - b) < 0.000001f) //包括出现负值时
{
}
else
{
motion_data1.DisplayTarget.y = Mathf.Sqrt(a - b) + motion_data2.Center_Point.y;
}
Debug.LogError("调试");
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
float a = (R - Radius2) * (R - Radius2);
float b = Mathf.Pow(motion_data2.DisplayStart.x - motion_data2.Center_Point.x, 2);
if ((a - b) < 0.000001f) //包括出现负值时
{
}
else
{
motion_data1.DisplayTarget.y = -Mathf.Sqrt(a - b) + motion_data2.Center_Point.y;
}
//Debug.LogError ("调试");
}
}
else //K1存在
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x) //右象限
{
float a = 1 + k1 * k1;
float b = -2 * motion_data2.Center_Point.x + 2 * k1 * (-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b1 - motion_data2.Center_Point.y);
float c = Mathf.Pow(motion_data2.Center_Point.x, 2) + Mathf.Pow(-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b1 - motion_data2.Center_Point.y, 2) - (R - Radius2) * (R - Radius2);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(b * b - 4 * a * c) - b) / (2 * a);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b1;
//"调试");
}
else //左象限
{
float a = 1 + k1 * k1;
float b = -2 * motion_data2.Center_Point.x + 2 * k1 * (-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b1 - motion_data2.Center_Point.y);
float c = Mathf.Pow(motion_data2.Center_Point.x, 2) + Mathf.Pow(-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b1 - motion_data2.Center_Point.y, 2) - (R - Radius2) * (R - Radius2);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(b * b - 4 * a * c) - b) / (2 * a);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b1;
//"调试");
}
}
}
protected override void FrameEndTrigger(MotionInfo motion)
{
_transformLock.Locked = false;
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2) //圆弧直线
{
float k2;
float b2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R = Vector3.Distance(motion_data1.Center_Point, motion_data1.DisplayTarget);
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x && motion_data2.DisplayTarget.y != motion_data2.DisplayStart.y) //k2不存在
{
if (motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
float a = (R - Radius1) * (R - Radius1);
float b = Mathf.Pow(motion_data2.DisplayStart.x - motion_data1.Center_Point.x, 2);
if ((a - b) < 0.000001f)
{
a = b;
}
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - Mathf.Sqrt(a - b);
//Debug.LogError ("调试");
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
float a = (R - Radius1) * (R - Radius1);
float b = Mathf.Pow(motion_data2.DisplayStart.x - motion_data1.Center_Point.x, 2);
if ((a - b) < 0.000001f)
{
a = b;
}
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + Mathf.Sqrt(a - b);
//Debug.LogError ("调试");
}
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
float dis01 = (k2 * (motion_data1.Center_Point.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - motion_data1.Center_Point.y) / Mathf.Sqrt(1 + k2 * k2);
float dis = Mathf.Abs(dis01);
if (dis == R)
{ //k2有正负，可不分情况
if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
}
else
{
if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y, 2) - (R - Radius1) * (R - Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b2;
//Debug.Log ("调试");
}
else
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y, 2) - (R - Radius1) * (R - Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b2;
//Debug.Log ("调试");
}
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2)
{
// float k1;
float k2;
float CE;
float AE;
float x0;
float y0;
float dis;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R1 = Vector3.Distance(motion_data2.DisplayStart, motion_data1.Center_Point);
float R2 = Vector3.Distance(motion_data2.Center_Point, motion_data2.DisplayStart);
dis = Vector3.Distance(motion_data1.Center_Point, motion_data2.Center_Point); //两圆心之间的距离
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x && motion_data1.DisplayTarget.y > motion_data1.Center_Point.y) //k1不存在
{
if (motion_data2.Center_Point.x == motion_data1.DisplayTarget.x)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (k2 == 0)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - AE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + CE;
//Debug.Log ("调试");
}
else
{
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
else if (motion_data1.Center_Point.x == motion_data1.DisplayTarget.x && motion_data1.Center_Point.y > motion_data1.DisplayTarget.y)
{
if (motion_data2.Center_Point.x == motion_data1.DisplayTarget.x)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (k2 == 0)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + AE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - CE;
//Debug.Log ("调试");
}
else
{
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
else
{
// k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y)/(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
// if (k1 > 0)
// {
if (motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
{
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (motion_data1.Center_Point.x == motion_data2.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
Debug.Log("调试"); //已检测
} //待检测
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (motion_data2.Center_Point.x < motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
else
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
}
}
else
{
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (motion_data1.Center_Point.x == motion_data2.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (motion_data2.Center_Point.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
else
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
}
}
// }
// else
// {
// if(motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
// {
// AE = ((R1 - Radius1)*(R1 - Radius1) - (R2 - Radius2)*(R2 - Radius2) + dis*dis )/(2*dis);
// CE = Mathf.Sqrt ((R1 - Radius1)*(R1 - Radius1) - AE*AE);
// if(motion_data1.Center_Point.x == motion_data2.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y)/(motion_data2.Center_Point.x - motion_data1.Center_Point.x);
// x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x)*AE/dis;
// y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x)*k2;
// if(motion_data2.Center_Point.x < motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = x0 - CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 + CE/Mathf.Sqrt (1 + k2*k2);
// //"调试");
// }
// else
// {
// motion_data1.DisplayTarget.x = x0 + CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 - CE/Mathf.Sqrt (1 + k2*k2);
// //"调试");
// }
// }
// }
// else
// {
// AE = ((R1 - Radius1)*(R1 - Radius1) - (R2 - Radius2)*(R2 - Radius2) + dis*dis )/(2*dis);
// CE = Mathf.Sqrt ((R1 - Radius1)*(R1 - Radius1) - AE*AE);
// if(motion_data1.Center_Point.x == motion_data2.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y)/(motion_data2.Center_Point.x - motion_data1.Center_Point.x);
// x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x)*AE/dis;
// y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x)*k2;
// if(motion_data2.Center_Point.x > motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = x0 + CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 - CE/Mathf.Sqrt (1 + k2*k2);
// //"调试");
// }
// else
// {
// motion_data1.DisplayTarget.x = x0 - CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 + CE/Mathf.Sqrt (1 + k2*k2);
// //"调试");
// }
// }
// }
// }
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2)
{
float k1;
float k2;
float b1;
float b2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x && motion_data1.DisplayTarget.x == motion_data2.DisplayTarget.x) //K1不存在,k2不存在
{
if (motion_data2.DisplayTarget.y > motion_data1.DisplayTarget.y && motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
Debug.LogError("测试");
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
Debug.LogError("测试");
}
}
else if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x && motion_data1.DisplayTarget.x != motion_data2.DisplayTarget.x) //K1不存在,k2存在
{
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y) //第一二点重合的时候
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b2;
//Debug.LogError ("测试");
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayStart.x - Radius1;
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b2;
//Debug.LogError ("测试");
}
}
else if (motion_data1.DisplayTarget.x == motion_data2.DisplayTarget.x && motion_data1.DisplayTarget.x != motion_data1.DisplayStart.x) //K2不存在,k1存在
{
if (motion_data2.DisplayTarget.y > motion_data1.DisplayTarget.y) //第一二点重合的时候
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
k1 = (motion_data2.DisplayStart.y - motion_data1.DisplayStart.y) / (motion_data2.DisplayStart.x - motion_data1.DisplayStart.x);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b1;
//Debug.LogError ("测试");
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
k1 = (motion_data2.DisplayStart.y - motion_data1.DisplayStart.y) / (motion_data2.DisplayStart.x - motion_data1.DisplayStart.x);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b1;
//Debug.LogError ("测试");
}
}
else //K2存在,k1存在
{
k1 = (motion_data2.DisplayStart.y - motion_data1.DisplayStart.y) / (motion_data2.DisplayStart.x - motion_data1.DisplayStart.x);
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
{
if (motion_data2.DisplayTarget.x > motion_data1.DisplayTarget.x)
{
motion_data1.DisplayTarget.x = (-b2 + b1) / (k1 - k2) + motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b1;
}
else
{
motion_data1.DisplayTarget.x = (b2 + b1) / (k1 - k2) + motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b1;
}
}
else
{
if (motion_data2.DisplayTarget.x > motion_data1.DisplayTarget.x)
{
motion_data1.DisplayTarget.x = (-b2 - b1) / (k1 - k2) + motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b1;
}
else
{
motion_data1.DisplayTarget.x = (b2 - b1) / (k1 - k2) + motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b1;
}
}
}
}
/// <summary>
/// Inicializace motoru
/// </summary>
/// <param name="connector">connector sběrnice</param>
/// <param name="stateObserver">posluchač stavu motoru</param>
/// <param name="nodeNumber">číslo node</param>
/// <param name="id">id motoru</param>
/// <param name="mode">defaultní nastavení módu</param>
/// <param name="reverse">příznak obrácení směru točení</param>
/// <param name="multiplier">násobitel otáček v případě, že je motor za převodovkou</param>
/// <param name="positionVeocity">rychlost motoru v otáčkách při pozicování</param>
/// <param name="positionAceleration">zrychlení motoru v otáčkách při pozicování</param>
/// <param name="positionDeceleration">zpomalení motoru v otáčkách při pozicování</param>
/// <param name="velocity">maximální rychlost motoru při rychlostním řízení</param>
/// <param name="aceleration">zrychlení motoru při rychlostním řízení</param>
/// <param name="deceleration">zpomalení motoru při rychlostním řízení</param>
public void inicialize(DeviceManager connector, IStateObserver stateObserver, Action motorErrorOccuredObserver, int nodeNumber, MotorId id, MotorMode mode, bool reverse, int multiplier, uint positionVelocity, uint positionAceleration, uint positionDeceleration, uint velocity, uint aceleration, uint deceleration)
{
try
{
this.mode = mode;
this.stateObserver = stateObserver;
this.motorErrorOccuredObserver = motorErrorOccuredObserver;
this.id = id;
this.multiplier = multiplier;
if (reverse)
{
rev = -1;
}
motor = connector.CreateDevice(Convert.ToUInt16(nodeNumber));
stateHandler = motor.Operation.MotionInfo;
sm = motor.Operation.StateMachine;
if (sm.GetFaultState())
sm.ClearFault();
sm.SetEnableState();
maxSpeed = (int)velocity;
velocityHandler = motor.Operation.ProfileVelocityMode;
velocityHandler.SetVelocityProfile(aceleration, deceleration);
positionHandler = motor.Operation.ProfilePositionMode;
positionHandler.SetPositionProfile(positionVelocity, positionAceleration, positionDeceleration);
homingHandler = motor.Operation.HomingMode;
changeMode(mode);
setStateObserver();
state = MotorState.enabled;
stateObserver.motorStateChanged(MotorState.enabled, "", id, 0, 0, 0, 0);
targetPosition = stateHandler.GetPositionIs();
targetAngle = getAngleFromPosition(targetPosition);
}
catch (DeviceException e)
{
sm = null;
disableStateObserver();
state = MotorState.error;
stateObserver.motorStateChanged(MotorState.error, String.Format("{0}\nError: {1}", e.ErrorMessage, errorDictionary.getComunicationErrorMessage(e.ErrorCode)), id, 0, 0, 0, 0);
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2) //直线圆弧
{
float k1;
float b1;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R = Vector3.Distance(motion_data1.DisplayTarget, motion_data2.Center_Point);
if (motion_data2.DisplayStart.x == motion_data1.DisplayStart.x)
{ //K1不存在
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y)
{
float a = motion_data2.Center_Point.y - motion_data2.DisplayStart.y;
if (a < 0.000001f && a > -0.000001f)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
float e = (R + Radius2) * (R + Radius2) - Mathf.Pow((motion_data1.DisplayTarget.x - motion_data2.Center_Point.x), 2);
if (e < 0.000001f)
{
e = 0;
}
motion_data1.DisplayTarget.y = -Mathf.Sqrt(e) + motion_data2.Center_Point.y;
//Debug.LogError ("调试");
}
}
else
{
float a = motion_data2.Center_Point.y - motion_data2.DisplayStart.y;
if (a < 0.000001f && a > -0.000001f)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
float e = (R + Radius2) * (R + Radius2) - Mathf.Pow((motion_data1.DisplayTarget.x - motion_data2.Center_Point.x), 2);
if (e < 0.000001f)
{
e = 0;
}
motion_data1.DisplayTarget.y = Mathf.Sqrt(e) + motion_data2.Center_Point.y;
//Debug.LogError ("调试");
}
}
}
else
{ //K1存在
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
{ //右象限
// Vector3 Direction = motion_data1.DisplayTarget - motion_data1.DisplayStart;
// Vector3 Direction2 = motion_data2.DisplayStart - motion_data2.Center_Point;
// float Angle ;
// Angle = Vector3.Angle (Direction, Direction2);
// if(Angle > 89.999f && Angle < 90.001f)
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius2*k1/Mathf.Sqrt (1 + k1*k1);
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius2/Mathf.Sqrt (1 + k1*k1);
// }
// else
// {
float a = 1 + k1 * k1;
float b = -2.0f * motion_data2.Center_Point.x + 2.0f * k1 * (-k1 * motion_data2.DisplayStart.x + motion_data2.DisplayStart.y - b1 - motion_data2.Center_Point.y);
float c = Mathf.Pow(motion_data2.Center_Point.x, 2.0f) + Mathf.Pow((-k1 * motion_data2.DisplayStart.x + motion_data2.DisplayStart.y - b1 - motion_data2.Center_Point.y), 2.0f) - (R + Radius2) * (R + Radius2);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b1;
Debug.LogError("调试"); //
// }
}
else
{ //左象限
// Vector3 Direction = motion_data1.DisplayTarget - motion_data1.DisplayStart;
// Vector3 Direction2 = motion_data2.DisplayStart - motion_data2.Center_Point;
// float Angle ;
// Angle = Vector3.Angle (Direction, Direction2);
//
// if(Angle > 89.999f && Angle < 90.001f)
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius2*k1/Mathf.Sqrt (1 + k1*k1);
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius2/Mathf.Sqrt (1 + k1*k1);
// }
// else
// {
float a = 1 + k1 * k1;
float b = -2 * motion_data2.Center_Point.x + 2 * k1 * (-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b1 - motion_data2.Center_Point.y);
float c = Mathf.Pow(motion_data2.Center_Point.x, 2) + Mathf.Pow(-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b1 - motion_data2.Center_Point.y, 2) - (R + Radius2) * (R + Radius2);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b1;
Debug.LogError("调试"); //
// }
}
}
}
/// <summary>
/// Code Check Entrance
/// </summary>
protected bool Code_Check(List<string> code_segment, int row_index, ref bool macro_flag, ref DataStore step_compile_data, ref Vector3 display_position, ref MotionInfo step_motion_data, ref Vector3 virtual_position)
{
bool temp_flag = true;
bool return_flag = true;
string Address = "";
Vector3 program_position = new Vector3(0, 0, 0);
macro_flag = false;
Regex macro_Reg = new Regex(@"((#+)|(\[+)|(\]+)|(=+))", RegexOptions.IgnoreCase);
MatchCollection macro_Col;
for(int i = 0; i < code_segment.Count; i++)
{
// 检查是否有宏代码，如果有宏代码
macro_Col = macro_Reg.Matches(code_segment[i]);
//如果程序中含有宏代码，中断编译过程，返回宏代码错误，宏代码的编译暂不处理
if(macro_Col.Count > 0)
{
temp_flag = false;
macro_flag = true;
break;
}
if(code_segment[i] != ";")
{
_errorMessage = "";
Address = code_segment[i][0].ToString().ToUpper();
switch(Address)
{
case "G":
return_flag = G_Check(code_segment[i], row_index, ref step_compile_data, ref ModalState);
break;
case "M":
return_flag = M_Check(code_segment[i], row_index, ref step_compile_data);
break;
/// A, B, C, I, J, K, U, V, W, X, Y, Z, R; F
case "A":
case "B":
case "C":
case "I":
case "J":
case "K":
case "U":
case "V":
case "W":
case "X":
case "Y":
case "Z":
case "R":
case "F":
return_flag = F_Check(code_segment[i], row_index, ref step_compile_data);
break;
case "/":
return_flag = Slash_Check(code_segment[i], row_index, ref step_compile_data);
break;
/// D, H; L, P; N, Q; O; S; T;
case "D":
case "H":
case "L":
case "P":
case "N":
case "Q":
case "O":
case "S":
case "T":
return_flag = I_Check(code_segment[i], row_index, ref step_compile_data);
break;
default:
_errorMessage = "(Line:" + row_index + "): " + Address + "地址不存在";
return_flag = false;
break;
}
if(return_flag == false)
{
if(temp_flag)
{
temp_flag = false;
}
_compileInfo.Add(ErrorMessage);
}
}
}
if(temp_flag)
{//compile level
//Todo: 分析此段代码，生成相关运动信息；
//是否为空
if(step_compile_data.IsEmpty())
return true;
else
{//1 level
//立即执行代码处理
if(step_compile_data.immediate_execution != "")
{//2 level
step_motion_data.Immediate_Motion = step_compile_data.immediate_execution;
for(int i = 0; i < step_compile_data.immediate_execution.Length; i++)
{
switch((char)step_compile_data.immediate_execution[i])
{
//Todo: 考虑下当前行只有T代码或者只有M06的情况
case (char)ImmediateMotionType.ToolChanging:
step_motion_data.Tool_Number = step_compile_data.tool_number;
break;
case (char)ImmediateMotionType.M03:
case (char)ImmediateMotionType.M04:
if(step_compile_data.s_value == 0)
{
_errorMessage = "(Line:" + row_index + "): " + "当前程序段中未指定主轴转速";
_compileInfo.Add(ErrorMessage);
return false;
}
step_motion_data.Rotate_Speed = step_compile_data.s_value;
break;
case (char)ImmediateMotionType.G54:
case (char)ImmediateMotionType.G55:
case (char)ImmediateMotionType.G56:
case (char)ImmediateMotionType.G57:
case (char)ImmediateMotionType.G58:
case (char)ImmediateMotionType.G59:
display_position = virtual_position - ModalState.LocalCoordinate();
break;
default:
break;
}
}
}//2 level
//计算运动信息
if(step_compile_data.HasMotion())
{//3 level
step_motion_data.Motion_Type = step_compile_data.MotionTypeIndex(ModalState.Modal_Code[0]);
if(step_motion_data.Motion_Type == -1)
{
_errorMessage = "(Line:" + row_index + "): " + "未知运动方式错误！";
_compileInfo.Add(ErrorMessage);
return false;
}
switch(step_motion_data.Motion_Type)
{
case (int)MotionType.DryRunning:
step_motion_data.SetStartPosition(display_position, virtual_position);
//公制单位
if(ModalState.UnitCheck() == (int)CheckInformation.MetricSystem)
{
//绝对坐标
if(ModalState.AbsoluteCooCheck() == (int)CheckInformation.AbsouteCoo)
{
program_position = step_compile_data.AbsolutePosition(display_position);
step_motion_data.Direction = program_position - display_position;
step_motion_data.SetRemainingMovement();
step_motion_data.Velocity = SystemArguments.RapidMoveSpeed;
step_motion_data.Time_Value = step_motion_data.Direction.magnitude / SystemArguments.RapidMoveSpeed * 60;
virtual_position += step_motion_data.Direction;
step_motion_data.SetTargetPosition(program_position, virtual_position);
}
//增量坐标
else
{
}
}
//英制单位
else
{
}
break;
case (int)MotionType.Line:
if(ModalState.Feedrate == 0)
{
_errorMessage = "(Line:" + row_index + "): " + "未指定进给速率！";
_compileInfo.Add(ErrorMessage);
return false;
}
step_motion_data.SetStartPosition(display_position, virtual_position);
//公制单位
if(ModalState.UnitCheck() == (int)CheckInformation.MetricSystem)
{
//绝对坐标
if(ModalState.AbsoluteCooCheck() == (int)CheckInformation.AbsouteCoo)
{
program_position = step_compile_data.AbsolutePosition(display_position);
step_motion_data.Direction = program_position - display_position;
step_motion_data.SetRemainingMovement();
step_motion_data.Velocity = ModalState.Feedrate;
step_motion_data.Time_Value = step_motion_data.Direction.magnitude / step_motion_data.Velocity * 60;
virtual_position += step_motion_data.Direction;
step_motion_data.SetTargetPosition(program_position, virtual_position);
}
//增量坐标
else
{
}
}
//英制单位
else
{
}
break;
case (int)MotionType.Circular02:
if(ModalState.Feedrate == 0)
{
_errorMessage = "(Line:" + row_index + "): " + "未指定进给速率！";
_compileInfo.Add(ErrorMessage);
return false;
}
step_motion_data.SetStartPosition(display_position, virtual_position);
//公制单位
if(ModalState.UnitCheck() == (int)CheckInformation.MetricSystem)
{
//绝对坐标
Vector3 xyz_coo = new Vector3(0, 0, 0);
Vector3 ijk_coo = new Vector3(0, 0, 0);
float rValue = 0;
if(ModalState.AbsoluteCooCheck() == (int)CheckInformation.AbsouteCoo)
{
switch(step_compile_data.CircleArguJudge(ref _errorMessage, ModalState, ref ijk_coo, ref xyz_coo, ref rValue))
{
case -1:
_errorMessage = "(Line:" + row_index + "): " + _errorMessage;
_compileInfo.Add(ErrorMessage);
return false;
case 0:
step_motion_data.Motion_Type = -1;
return true;
case 1:
step_motion_data.Center_Point = display_position + ijk_coo;
step_motion_data.Rotate_Degree = 360f;
break;
}
program_position = step_compile_data.AbsolutePosition(display_position);
step_motion_data.Direction = program_position - display_position;
step_motion_data.SetRemainingMovement();
step_motion_data.Velocity = ModalState.Feedrate;
step_motion_data.Time_Value = step_motion_data.Direction.magnitude / step_motion_data.Velocity * 60;
virtual_position += step_motion_data.Direction;
step_motion_data.SetTargetPosition(program_position, virtual_position);
}
//增量坐标
else
{
}
}
//英制单位
else
{
}
break;
case (int)MotionType.Circular03:
break;
default:
_errorMessage = "(Line:" + row_index + "): " + "当前系统暂不支持Motion Type: " + step_compile_data.motion_type;
_compileInfo.Add(ErrorMessage);
return false;
}
}//3 level
}//1 level
if(step_compile_data.slash_value > 0)
{
if(ModalState.Slash)
return true;
}
return true;
}//compile level
else
return false;
}
/// <summary>
/// The main method of NC code compile
/// </summary>
/// <returns>
/// compile result type
/// </returns>
/// <param name='source_code'>
/// formative NC code
/// </param>
/// <param name='prog_name'>
/// the name of current NC program
/// </param>
public int CompileEntrance(List<List<string>> source_code, string prog_name, ref List<DataStore> compile_data, Vector3 current_position, ref List<MotionInfo> motion_data, Vector3 virtual_position)
{
//初始信息
bool temp_execute = true;
bool macro_flag = false;
ResultType type_var = ResultType.Success;
_executeFlag = false;
ErrorClear();
compile_data.Clear();
motion_data.Clear();
DataStore step_compile_data = new DataStore();
MotionInfo step_motion_data = new MotionInfo();
for(int i = 0; i < source_code.Count; i++)
{
step_compile_data = new DataStore();
step_motion_data = new MotionInfo();
if(Code_Check(source_code[i], i + 1, ref macro_flag, ref step_compile_data, ref current_position, ref step_motion_data, ref virtual_position) == false)
{
if(macro_flag)
{
temp_execute = false;
type_var = ResultType.MacroError;
compile_data.Clear();
break;
}
if(temp_execute)
{
temp_execute = false;
type_var = ResultType.CompileError;
}
}
if(temp_execute)
{
compile_data.Add(step_compile_data);
motion_data.Add(step_motion_data);
}
}
if(temp_execute)
_executeFlag = true;
else
{
_executeFlag = false;
compile_data.Clear();
motion_data.Clear();
}
int return_type = (int)type_var;
return return_type;
}
//
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2)
{
float k1;
float k2;
float b1;
float b2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
//K1不存在,k2不存在
if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x && motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{ //档半径变化时，补偿半径12可能会发生变化
if (motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y && motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1;
motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y;
}
else
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius1;
motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y;
}
}
else if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x && motion_data2.DisplayStart.x != motion_data2.DisplayTarget.x) //K1不存在,k2存在
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y) //第一条直线向上
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1;
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b2;
}
else //第一条直线向下
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius1;
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b2;
}
}
else if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x && motion_data2.DisplayStart.x != motion_data1.DisplayStart.x) //K2不存在,k1存在
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y) //第二条直线方向向上
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b1;
}
else //第二条直线向下
{
//调试过Debug.LogWarning ("GG");
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b1;
}
}
else //K2存在,k1存在
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x) //右侧象限
{
// "调试");
if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
motion_data1.DisplayTarget.x = (b2 - b1) / (k1 - k2) + motion_data2.DisplayStart.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b1;
}
else
{
motion_data1.DisplayTarget.x = (-b2 - b1) / (k1 - k2) + motion_data2.DisplayStart.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b1;
}
}
else //左侧象限
{
if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
motion_data1.DisplayTarget.x = (b2 + b1) / (k1 - k2) + motion_data2.DisplayStart.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b1;
}
else
{
motion_data1.DisplayTarget.x = (-b2 + b1) / (k1 - k2) + motion_data2.DisplayStart.x;
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b1;
}
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2)
{
float k1;
float k2;
float Up_Down = 0;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
//保证圆弧连接型的圆心坐标为发生改变
motion_data_circle.Center_Point = motion_data1.DisplayTarget;
if (motion_data1.DisplayTarget.y == motion_data1.Center_Point.y)
{
if (motion_data2.DisplayStart.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
}
else
{
k2 = (motion_data2.DisplayStart.y - motion_data2.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data2.Center_Point.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
} //待测试
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
}
else
{
k2 = (motion_data2.DisplayStart.y - motion_data2.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data2.Center_Point.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
}
}
}
else
{
if (motion_data1.DisplayTarget.y > motion_data1.Center_Point.y) //第一段圆弧终点和圆心所在直线方向向上
{
Up_Down = 1;
}
else
{
Up_Down = -1;
}
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x) //第一段圆弧终点和圆心所在直线的斜率不存在
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * Up_Down;
if (motion_data2.Center_Point.x == motion_data1.Center_Point.x) //圆心连线的斜率不存在
{
motion_data_circle.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data_circle.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius2 * Up_Down;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Up_Down / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 * Up_Down / Mathf.Sqrt(1 + k2 * k2);
}
}
else
{
k1 = (motion_data2.DisplayStart.y - motion_data1.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data1.Center_Point.x);
if (motion_data1.Center_Point.x < motion_data2.DisplayStart.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data1.Center_Point.x == motion_data2.Center_Point.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayTarget.y - Radius2;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data1.Center_Point.x == motion_data2.Center_Point.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayTarget.y + Radius2;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2)
{
// float k1;
float k2;
float CE;
float AE;
float x0;
float y0;
float dis;
float R1 = Vector3.Distance(motion_data1.DisplayTarget, motion_data1.Center_Point);
float R2 = Vector3.Distance(motion_data2.DisplayTarget, motion_data2.Center_Point);
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
dis = Vector3.Distance(motion_data1.Center_Point, motion_data2.Center_Point); //两圆心坐标之间的距离
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x && motion_data1.DisplayTarget.y > motion_data1.Center_Point.y) //第一段终点和对应所在直线的k1不存在，并且方向向上
{
if (motion_data2.Center_Point.x == motion_data1.DisplayTarget.x && motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1;
//Debug.LogError ("调试");
}
else if (motion_data2.Center_Point.x == motion_data1.DisplayTarget.x && motion_data2.DisplayTarget.y < motion_data2.DisplayStart.y)
{
AE = ((R1 + Radius1) * (R1 + Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2.0F * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
//Debug.LogError ("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
AE = ((R1 + Radius1) * (R1 + Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (k2 == 0)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - AE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + CE;
//Debug.LogError ("调试");
}
else
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
}
}
else if (motion_data1.Center_Point.x == motion_data1.DisplayTarget.x && motion_data1.Center_Point.y > motion_data1.DisplayTarget.y) //第一段终点与对应圆心所在直线k1不存在并且方向向下
{
//左侧象限是有效区域
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x && motion_data2.DisplayTarget.y < motion_data2.DisplayStart.y) //第二段起点和对应圆心所在直线的k2不存在
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1;
Debug.LogError("调试");
}
else if (motion_data2.Center_Point.x == motion_data2.DisplayTarget.x && motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y)
{
AE = ((R1 + Radius1) * (R1 + Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + CE;
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.y + AE;
Debug.LogError("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
AE = ((R1 + Radius1) * (R1 + Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
CE = Mathf.Sqrt((R1 + Radius1) * (R1 + Radius1) - AE * AE);
x0 = motion_data1.Center_Point.x - (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (k2 < 0.000001f && k2 > -0.000001f)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + AE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - CE;
//Debug.LogError ("调试");
}
else
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试");
}
}
}
else //第一段终点和对应圆心所在直线的K1存在
{
// k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y)/(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
// if (k1 > 0)
// {
if (motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
{
AE = ((R1 + Radius1) * (R1 + Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 + Radius1) * (R1 + Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (motion_data2.Center_Point.x == motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
//Debug.LogError ("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (motion_data2.Center_Point.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
else
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试");
}
}
}
else
{
AE = ((R1 + Radius1) * (R1 + Radius1) - (R2 - Radius2) * (R2 - Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (motion_data2.Center_Point.x == motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
Debug.LogError("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (motion_data2.Center_Point.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
else
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试");
}
}
}
// }
// else//k1 < 0
// {
// if(motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
// {
// AE = ((R1 + Radius1)*(R1 + Radius1) - (R2 - Radius2)*(R2 - Radius2) + dis*dis )/(2*dis);
// CE = Mathf.Sqrt ((R1 + Radius1)*(R1 + Radius1) - AE*AE);
//
// if(motion_data2.Center_Point.x == motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
// //调试过
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y)/(motion_data2.Center_Point.x - motion_data1.Center_Point.x);
// x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x)*AE/dis;
// y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x)*k2;
//
// if(motion_data2.Center_Point.x > motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = x0 + CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 - CE/Mathf.Sqrt (1 + k2*k2);
// //Debug.LogError ("调试");
// }
// else
// {
// motion_data1.DisplayTarget.x = x0 - CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 + CE/Mathf.Sqrt (1 + k2*k2);
// Debug.LogError ("调试");
// }
// }
// }
// else
// {
// AE = ((R1 + Radius1)*(R1 + Radius1) - (R2 - Radius2)*(R2 - Radius2) + dis*dis )/(2*dis);
// CE = Mathf.Sqrt ((R1 + Radius1)*(R1 + Radius1) - AE*AE);
// if(motion_data2.Center_Point.x == motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y)/(motion_data2.Center_Point.x - motion_data1.Center_Point.x);
// x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x)*AE/dis;
// y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x)*k2;
//
// if(motion_data2.Center_Point.x > motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = x0 + CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 - CE/Mathf.Sqrt (1 + k2*k2);
// Debug.LogError ("调试");
// }
// else
// {
// motion_data1.DisplayTarget.x = x0 - CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 + CE/Mathf.Sqrt (1 + k2*k2);
// //Debug.LogError ("调试");
// }
// }
// }
// }
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2)
{
// float k1;
float k2;
float CE;
float AE;
float x0;
float y0;
float dis;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R1 = Vector3.Magnitude(motion_data1.DisplayTarget - motion_data1.Center_Point);
float R2 = Vector3.Magnitude(motion_data2.DisplayTarget - motion_data2.Center_Point);
dis = Mathf.Sqrt(Mathf.Pow(motion_data1.Center_Point.y - motion_data2.Center_Point.y, 2) + Mathf.Pow(motion_data1.Center_Point.x - motion_data2.Center_Point.x, 2));
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x && motion_data1.DisplayTarget.y > motion_data1.Center_Point.y) //k1不存在且方向向上
{
if (motion_data2.Center_Point.x == motion_data1.DisplayTarget.x)
{
if (motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1;
//Debug.LogError ("调试");
}
else
{
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 + Radius2) * (R2 + Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
//Debug.LogError ("调试");
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 + Radius2) * (R2 + Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (k2 == 0)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + AE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + CE;
//Debug.LogError ("调试");
}
else
{
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
}
}
else if (motion_data1.Center_Point.x == motion_data1.DisplayTarget.x && motion_data1.Center_Point.y > motion_data1.DisplayTarget.y) //k1不存在且方向向下
{
if (motion_data2.Center_Point.x == motion_data1.DisplayTarget.x)
{
if (motion_data2.Center_Point.y < motion_data2.DisplayStart.y)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1;
//Debug.LogError ("调试");
}
else
{
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 + Radius2) * (R2 + Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
//Debug.LogError ("调试");
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 + Radius2) * (R2 + Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
if (k2 == 0)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - AE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - CE;
//Debug.LogError ("调试");
}
else
{
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
//Debug.Log (x0 + "," + y0);
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.LogError ("调试");
}
}
}
else //k1存在
{
// k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y)/(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
// float y2 = k1*(motion_data2.Center_Point.x - motion_data1.DisplayTarget.x) + motion_data1.DisplayTarget.y;
// if (k1 > 0)
// {
if (motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
{
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 + Radius2) * (R2 + Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
float a = motion_data1.Center_Point.x - motion_data2.Center_Point.x;
if (a > -0.0001f && a < 0.0001f)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
//Debug.Log ("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (motion_data2.Center_Point.x < motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
}
}
else
{
AE = ((R1 - Radius1) * (R1 - Radius1) - (R2 + Radius2) * (R2 + Radius2) + dis * dis) / (2 * dis);
float CE_Quare = (R1 - Radius1) * (R1 - Radius1) - AE * AE;
if (CE_Quare < 0)
{
CE_Quare = 0;
}
CE = Mathf.Sqrt(CE_Quare);
float a = motion_data1.Center_Point.x - motion_data2.Center_Point.x;
if (a > -0.0001 && a < 0.0001)
{
motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
//Debug.Log ("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y) / (motion_data2.Center_Point.x - motion_data1.Center_Point.x);
x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x) * AE / dis;
y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x) * k2;
if (motion_data2.Center_Point.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = x0 - CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 + CE / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
motion_data1.DisplayTarget.x = x0 + CE * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data1.DisplayTarget.y = y0 - CE / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
// }
// else//小于等于o
// {
// if(motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
// {
// AE = ((R1 - Radius1)*(R1 - Radius1) - (R2 + Radius2)*(R2 + Radius2) + dis*dis )/(2*dis);
// CE = Mathf.Sqrt ((R1 - Radius1)*(R1 - Radius1) - AE*AE);
// float a = motion_data1.Center_Point.x - motion_data2.Center_Point.x;
// if(a > -0.0001 && a < 0.0001)
// {
// motion_data1.DisplayTarget.x = motion_data1.Center_Point.x + CE;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - AE;
// //Debug.Log ("调试");
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y)/(motion_data2.Center_Point.x - motion_data1.Center_Point.x);
// x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x)*AE/dis;
// y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x)*k2;
// if(motion_data2.Center_Point.x < motion_data1.Center_Point.x)
// {
// Debug.Log (x0 + "," + y0);
// motion_data1.DisplayTarget.x = x0 + CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 - CE/Mathf.Sqrt (1 + k2*k2);
// Debug.Log ("调试");
// }
// else
// {
// motion_data1.DisplayTarget.x = x0 - CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 + CE/Mathf.Sqrt (1 + k2*k2);
// //Debug.Log ("调试");
// }
// }
//
// }
// else
// {
// AE = ((R1 - Radius1)*(R1 - Radius1) - (R2 + Radius2)*(R2 + Radius2) + dis*dis )/(2*dis);
// CE = Mathf.Sqrt ((R1 - Radius1)*(R1 - Radius1) - AE*AE);
// float a = motion_data1.Center_Point.x - motion_data2.Center_Point.x;
// if(a > -0.0001 && a < 0.0001)
// {
// motion_data1.DisplayTarget.x = motion_data1.Center_Point.x - CE;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + AE;
// Debug.Log ("调试");
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data1.Center_Point.y)/(motion_data2.Center_Point.x - motion_data1.Center_Point.x);
// x0 = motion_data1.Center_Point.x + (motion_data2.Center_Point.x - motion_data1.Center_Point.x)*AE/dis;
// y0 = motion_data1.Center_Point.y + (x0 - motion_data1.Center_Point.x)*k2;
//
// if(motion_data2.Center_Point.x > motion_data1.Center_Point.x)
// {
// motion_data1.DisplayTarget.x = x0 - CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 + CE/Mathf.Sqrt (1 + k2*k2);
// //"调试");
// }
// else
// {
// motion_data1.DisplayTarget.x = x0 + CE*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data1.DisplayTarget.y = y0 - CE/Mathf.Sqrt (1 + k2*k2);
// Debug.Log ("调试");
// }
// }
// }
// }
}
}
protected override void FrameStartTrigger(MotionInfo motion)
{
_transformLock.Locked = true;
entity.setLocalPosition(motion.Offset);
entity.setLocalRotation(motion.Angle);
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2)
{
float k1;
float k2;
float Up_Down = 0;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
//保证圆弧连接型的圆心坐标为发生改变
// motion_data_circle.Center_Point = motion_data1.DisplayTarget;
if (motion_data1.DisplayTarget.y == motion_data1.Center_Point.y)
{
if (motion_data2.DisplayStart.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
float e = motion_data2.DisplayStart.y - motion_data2.Center_Point.y;
if (e < 0.00001 && e > -0.00001)
{
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.LogError ("调试");
}
}
else
{
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
//Debug.LogError ("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (k2 > 0)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ( "调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
}
}
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
float e = motion_data2.DisplayStart.y - motion_data2.Center_Point.y;
if (e < 0.00001 && e > -0.00001)
{
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.LogError ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
}
}
else
{
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
//Debug.LogError ("调试");
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (k2 > 0)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ( "调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
}
}
}
}
else
{
if (motion_data1.DisplayTarget.y > motion_data1.Center_Point.y) //读一段圆弧终点和圆心所在的圆弧的直线方向向上
{
Up_Down = 1;
}
else
{
Up_Down = -1;
}
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x) //读一段圆弧终点和圆心所在的圆弧的直线的斜率不存在
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 * Up_Down;
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x && motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayTarget.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayTarget.y - Radius2;
}
else if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x && motion_data2.Center_Point.y <= motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
//Debug.Log ( "调试");
}
else
{
Debug.Log(Up_Down);
k2 = (motion_data2.DisplayStart.y - motion_data2.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data2.Center_Point.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Up_Down / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * Up_Down * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ( "调试");//待检测//已检测
}
}
else
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y) / (motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
if (motion_data2.DisplayStart.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
//Debug.Log ( "调试");
}
else
{
k2 = (motion_data2.DisplayStart.y - motion_data2.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data2.Center_Point.x);
if (motion_data2.Center_Point.x < motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
// "调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ( "调试");
}
}
}
else
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
}
else
{
k2 = (motion_data2.DisplayStart.y - motion_data2.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data2.Center_Point.x);
if (motion_data2.Center_Point.x < motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
// "调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
}
}
}
}
}
private MotionInfo GetMotionInfo(Mat image)
{
Mat _forgroundMask = new Mat();
Mat _segMask = new Mat();
MotionInfo motionInfoObj = new MotionInfo();
double minArea, angle, objectCount, totalPixelCount;
double overallangle = 0;
double motionPixelCount =0;
int motionArea =0;
totalPixelCount = 0;
objectCount = 0;
minArea = 800;
if (foregroundDetector == null)
{
foregroundDetector = new BackgroundSubtractorMOG2();
}
foregroundDetector.Apply(image, _forgroundMask);
_motionHistory.Update(_forgroundMask);
ImageForeGroundMaskLast = _forgroundMask.ToImage<Bgr, byte>();
#region get a copy of the motion mask and enhance its color
double[] minValues, maxValues;
Point[] minLoc, maxLoc;
_motionHistory.Mask.MinMax(out minValues, out maxValues, out minLoc, out maxLoc);
Mat motionMask = new Mat();
using (ScalarArray sa = new ScalarArray(255.0 / maxValues[0]))
CvInvoke.Multiply(_motionHistory.Mask, sa, motionMask, 1, DepthType.Cv8U);
//Image<Gray, Byte> motionMask = _motionHistory.Mask.Mul(255.0 / maxValues[0]);
#endregion
//create the motion image
Image<Bgr, Byte> motionImage = new Image<Bgr, byte>(motionMask.Size);
//display the motion pixels in blue (first channel)
//motionImage[0] = motionMask;
CvInvoke.InsertChannel(motionMask, motionImage, 0);
//Threshold to define a motion area, reduce the value to detect smaller motion
minArea = 100;
//storage.Clear(); //clear the storage
Rectangle[] rects;
using (VectorOfRect boundingRect = new VectorOfRect())
{
_motionHistory.GetMotionComponents(_segMask, boundingRect);
rects = boundingRect.ToArray();
}
//iterate through each of the motion component
foreach (Rectangle comp in rects)
{
int area = comp.Width * comp.Height;
//reject the components that have small area;
_motionHistory.MotionInfo(_forgroundMask, comp, out angle, out motionPixelCount);
if (area < minArea) continue;
else
{
overallangle = overallangle + angle;
totalPixelCount = totalPixelCount + motionPixelCount;
objectCount = objectCount + 1;
motionArea = motionArea + area;
}
// find the angle and motion pixel count of the specific area
////Draw each individual motion in red
//DrawMotion(motionImage, comp, angle, new Bgr(Color.Red));
}
motionInfoObj.MotionArea = motionArea;
motionInfoObj.OverallAngle = overallangle;
motionInfoObj.BoundingRect = rects;
motionInfoObj.TotalMotions = rects.Length;
motionInfoObj.MotionObjects = objectCount;
motionInfoObj.MotionPixels = totalPixelCount;
averagetotalPixelCount = 0.75 * averagetotalPixelCount + 0.25 * totalPixelCount;
if ( Math.Abs(averagetotalPixelCount - totalPixelCount) / averagetotalPixelCount > 0.59)
Console.WriteLine(" GetMotionInfo - Total Motions found: " + rects.Length + "; Motion Pixel count: " + totalPixelCount);
return motionInfoObj;
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2) //圆弧直线G02G01内角
{
float k2;
float b2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R = Vector3.Distance(motion_data1.Center_Point, motion_data1.DisplayStart);
//第二段直线的斜率不存在
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x && motion_data2.DisplayTarget.y != motion_data2.DisplayStart.y)
{
if (motion_data2.DisplayTarget.y > motion_data1.DisplayTarget.y) //第二条直线垂直向上
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
float a = (R + Radius2) * (R + Radius2);
float b = Mathf.Pow(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x, 2);
if ((a - b) < 0.000001f) //保证精度
{
a = b;
}
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - Mathf.Sqrt(a - b);
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
float a = (R + Radius2) * (R + Radius2);
float b = Mathf.Pow(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x, 2);
if ((a - b) < 0.000001f)
{
a = b;
}
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + Mathf.Sqrt(a - b);
}
}
else //第二段直线的斜率存在
{
//k2存在
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b2;
}
else
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b2;
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2)
{
float k1;
float k2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
if (motion_data2.DisplayStart.x == motion_data1.DisplayStart.x) //第一段直线的斜率k1不存在
{
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x && motion_data2.DisplayTarget.y < motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试"); //
}
}
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试"); //
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试");
}
}
}
}
else //k1存在
{
k1 = (motion_data2.DisplayStart.y - motion_data1.DisplayStart.y) / (motion_data2.DisplayStart.x - motion_data1.DisplayStart.x);
// if(k1 == 0)
// {
// if(motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1;
//
// if(motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
// //Debug.LogError ("调试");
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y)/(motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2/Mathf.Sqrt(1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2*k2/Mathf.Sqrt(1 + k2*k2);
// //Debug.LogError ("调试");
// }
// }
// else
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1;
//
// if(motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
// //Debug.LogError ("调试");
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y)/(motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2/Mathf.Sqrt(1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2*k2/Mathf.Sqrt(1 + k2*k2);
// //Debug.LogError ("调试");
// }
// }
// }
// else if(k1 > 0)
// {
// if(motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1*k1/Mathf.Sqrt(1 + k1*k1);
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1/Mathf.Sqrt(1 + k1*k1);
// //Debug.LogError ("调试");
// if(motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y)/(motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
// if(motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2/Mathf.Sqrt(1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2*k2/Mathf.Sqrt(1 + k2*k2);
// //"调试");
// }
// else
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2/Mathf.Sqrt(1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2*k2/Mathf.Sqrt(1 + k2*k2);
// //"调试");
// }
// }
// }
// else
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1*k1/Mathf.Sqrt(1 + k1*k1);
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1/Mathf.Sqrt(1 + k1*k1);
// //Debug.LogError ("调试");
// if(motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
// }
// else
// {
// k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y)/(motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
// if(motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2/Mathf.Sqrt(1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2*k2/Mathf.Sqrt(1 + k2*k2);
// //Debug.LogError ("调试");
// }
// else
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2/Mathf.Sqrt(1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2*k2/Mathf.Sqrt(1 + k2*k2);
// //"调试");
// }
// }
// }
// }
// else//K1小于零
{
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
if (motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
Debug.LogError("调试"); //
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
Debug.LogError("调试"); //
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试"); //
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试"); //
}
}
}
else
{ //Debug.LogError ("调试");
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 / Mathf.Sqrt(1 + k1 * k1);
//("调试");
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
if (motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
Debug.LogError("调试"); //
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
Debug.LogError("调试"); //
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试"); //
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.LogError("调试"); //
}
}
}
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2)
{
float k1;
float k2;
float Radius1;
float Radius2;
float Up_Down = 0;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x) //第一段圆弧终点和圆心的直线k1不存在
{
if (motion_data1.DisplayTarget.y > motion_data1.Center_Point.y)
{
Up_Down = 1;
}
else
{
Up_Down = -1;
}
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 * Up_Down;
if (motion_data2.DisplayTarget.y == motion_data1.DisplayTarget.y && motion_data2.DisplayTarget.x < motion_data1.DisplayStart.x)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius2;
}
else if (motion_data2.DisplayTarget.y == motion_data1.DisplayTarget.y && motion_data2.DisplayTarget.x > motion_data1.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
Debug.Log("调试");
}
else
{
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * Up_Down;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.Log ("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
}
else if (motion_data1.DisplayTarget.y == motion_data1.Center_Point.y) //第一段圆弧半径所在的直线的斜率为零
{
if (motion_data1.DisplayTarget.x < motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y ;
Debug.Log("调试");
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y ;
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
}
else //第一段圆弧终点处半径所在的直线斜率不为零且存在
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y) / (motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
// if(k1 > 0)
// {
if (motion_data1.DisplayTarget.x > motion_data1.Center_Point.x) //偏向右象限
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
//Debug.Log ("调试");
if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{
if (motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
if (motion_data2.DisplayStart.x < motion_data2.DisplayTarget.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
else //偏向左象限
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{
if (motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
}
else if (motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
}
// }
// else
// {
// if(motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)//偏向右侧象限
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1/Mathf.Sqrt (1 + k1*k1);
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1*k1/Mathf.Sqrt (1 + k1*k1);
// //Debug.Log ("调试");
// if(motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
// //"调试");
// }
// else
// {
// k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y)/(motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
// if(motion_data2.DisplayStart.x < motion_data2.DisplayTarget.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2/Mathf.Sqrt (1 + k2*k2);
// }
// else
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2/Mathf.Sqrt (1 + k2*k2);
// //Debug.Log ("调试");
// }
// }
// }
// else//偏向左侧象限
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1/Mathf.Sqrt (1 + k1*k1);
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1*k1/Mathf.Sqrt (1 + k1*k1);
// //Debug.Log ("调试");
//
// if(motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
// {
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
// Debug.Log ("调试");
// }
// else if(motion_data2.DisplayTarget.x > motion_data2.DisplayStart.x)
// {
// k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y)/(motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2/Mathf.Sqrt (1 + k2*k2);
// Debug.Log ("调试");
// }
// else
// {
// k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y)/(motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
// //Debug.Log ("调试");
// motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2*k2/Mathf.Sqrt (1 + k2*k2);
// motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2/Mathf.Sqrt (1 + k2*k2);
// }
// }
// }
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2)
{
// float k1;
float k2;
// float b1;
float b2;
float Radius1;
float Radius2;
float Up_Down;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R = Vector3.Distance(motion_data1.Center_Point, motion_data1.DisplayStart);
float e = motion_data1.Center_Point.y - motion_data2.DisplayStart.y;
if (e < 0.0001f && e > -0.0001f) //精度可能要重新考虑
{
if (motion_data1.Center_Point.x < motion_data2.DisplayStart.x)
{
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
if (motion_data2.DisplayStart.y < motion_data2.DisplayTarget.y)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius1; //补偿半径不同时
motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.Log ("调试");
}
else
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1; //补偿半径不同时
float f = (R + Radius1) * (R + Radius1) - Mathf.Pow(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x, 2);
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - Mathf.Sqrt(f);
//Debug.Log ("调试1");
}
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b2;
//Debug.Log ("调试1");
}
}
else
{
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
if (motion_data2.DisplayStart.y > motion_data2.DisplayTarget.y)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x - Radius1; //补偿半径不同时
motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
else
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius1; //补偿半径不同时
float f = (R + Radius1) * (R + Radius1) - Mathf.Pow(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x, 2);
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + Mathf.Sqrt(f);
Debug.Log("调试"); //待测试
}
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b2;
//Debug.Log ("调试1");
}
}
}
else
{
if (motion_data1.Center_Point.y < motion_data2.DisplayStart.y)
{
Up_Down = 1;
}
else
{
Up_Down = -1;
}
if (motion_data1.Center_Point.x == motion_data2.DisplayStart.x)
{
if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{
motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Up_Down;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + Up_Down * Mathf.Sqrt((R + Radius1) * (R + Radius1) - Mathf.Pow(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x, 2));
//Debug.Log ("测试12");
}
else
{
// if(motion_data2.DisplayStart.y == motion_data2.DisplayTarget.y)
// {
// motion_data1.DisplayTarget.x = motion_data2.DisplayStart.x;
// motion_data1.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2*Up_Down;
// Debug.LogError ("调试");
// }
// else
// {
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
if (motion_data2.DisplayStart.x < motion_data2.DisplayTarget.x)
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b2;
//Debug.LogError ("调试");
}
else
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b2;
//Debug.LogError ("调试");
}
// }
}
}
else
{
// k1 = (motion_data2.DisplayStart.y - motion_data1.DisplayStart.y)/(motion_data2.DisplayStart.x - motion_data1.DisplayStart.x);
// b1 = Radius1*Mathf.Sqrt (1 + k1*k1);
if (motion_data1.Center_Point.x < motion_data2.DisplayStart.x)
{
if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{
// if(motion_data2.DisplayStart.y < motion_data1.Center_Point.y)
// {
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius2;
// motion_data1.DisplayTarget.y = motion_data1.Center_Point.y - Mathf.Sqrt ((R + Radius1)*(R + Radius1) - Mathf.Pow (motion_data1.DisplayStart.x - motion_data1.Center_Point.x,2));
// //Debug.Log ("调试");
// }
// else
// {
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius2 * Up_Down;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + Up_Down * Mathf.Sqrt((R + Radius1) * (R + Radius1) - Mathf.Pow(motion_data1.DisplayStart.x - motion_data1.Center_Point.x, 2));
Debug.Log("测试1");
// }
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
if (motion_data2.DisplayStart.x < motion_data2.DisplayTarget.x)
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data2.DisplayTarget.x + motion_data2.DisplayTarget.y - b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data2.DisplayTarget.x + motion_data2.DisplayTarget.y - b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
//Debug.Log (d);
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b2;
Debug.Log("调试1");
}
else
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
//Debug.Log (d);
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b2;
//Debug.Log ("调试1");
}
}
}
else
{ //Debug.Log ("测试123");
if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius2 * Up_Down;
motion_data1.DisplayTarget.y = motion_data1.Center_Point.y + Up_Down * Mathf.Sqrt((R + Radius1) * (R + Radius1) - Mathf.Pow(motion_data1.DisplayTarget.x - motion_data1.Center_Point.x, 2));
//Debug.Log ("测试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
b2 = Radius2 * Mathf.Sqrt(1 + k2 * k2);
if (motion_data2.DisplayStart.x < motion_data2.DisplayTarget.x)
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
//Debug.Log (d);
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y - b2;
//Debug.Log ("调试");
}
else
{
float a = 1 + k2 * k2;
float b = -2 * motion_data1.Center_Point.x + 2 * k2 * (-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y);
float c = Mathf.Pow(motion_data1.Center_Point.x, 2) + Mathf.Pow(-k2 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b2 - motion_data1.Center_Point.y, 2) - (R + Radius1) * (R + Radius1);
float d = b * b - 4 * a * c;
//Debug.Log (d);
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k2 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data2.DisplayStart.y + b2;
//Debug.Log ("调试");
}
}
}
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data2) //直线圆弧G03内角
{
float k1;
float b1;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
float R = Vector3.Distance(motion_data2.DisplayStart, motion_data2.Center_Point);
if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x) //第一条直线K1不存在
{
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y) //第一条直线的矢量方向向上
{
if (motion_data2.Center_Point.y == motion_data1.DisplayTarget.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
} //怕容易出现平方中是负值，相等时作特别计算
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
float a = (R - Radius2) * (R - Radius2);
float b = Mathf.Pow(motion_data1.DisplayTarget.x - motion_data2.Center_Point.x, 2);
if (a - b < 0.000001f)
{
a = b;
}
motion_data1.DisplayTarget.y = Mathf.Sqrt(a - b) + motion_data2.Center_Point.y;
//Debug.Log ("调试");
}
}
else
{
//"调试");
if (motion_data2.Center_Point.y == motion_data1.DisplayTarget.y)
{
//"调试");
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
} //怕容易出现平方中是负值，相等时作特别计算
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
float a = (R - Radius2) * (R - Radius2);
float b = Mathf.Pow(motion_data1.DisplayTarget.x - motion_data2.Center_Point.x, 2);
if (a - b < 0.000001f)
{
a = b;
}
motion_data1.DisplayTarget.y = -Mathf.Sqrt(a - b) + motion_data2.Center_Point.y;
//Debug.Log ("调试");
}
}
}
else //第一条直线K1存在
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
b1 = Radius1 * Mathf.Sqrt(1 + k1 * k1);
//调试过
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
{ //右象限
float a = 1 + k1 * k1;
float b = -2 * motion_data2.Center_Point.x + 2 * k1 * (-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b1 - motion_data2.Center_Point.y);
float c = Mathf.Pow(motion_data2.Center_Point.x, 2) + Mathf.Pow(-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y + b1 - motion_data2.Center_Point.y, 2) - (R - Radius2) * (R - Radius2);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
//Debug.Log (d + " " + R);
motion_data1.DisplayTarget.x = (Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y + b1;
//Debug.Log ("调试");
}
else
{ //左象限
float a = 1 + k1 * k1;
float b = -2 * motion_data2.Center_Point.x + 2 * k1 * (-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b1 - motion_data2.Center_Point.y);
float c = Mathf.Pow(motion_data2.Center_Point.x, 2) + Mathf.Pow(-k1 * motion_data1.DisplayTarget.x + motion_data1.DisplayTarget.y - b1 - motion_data2.Center_Point.y, 2) - (R - Radius2) * (R - Radius2);
float d = b * b - 4 * a * c;
if (d < 0.000001f)
{
d = 0;
}
motion_data1.DisplayTarget.x = (-Mathf.Sqrt(d) - b) / (2 * a);
motion_data1.DisplayTarget.y = k1 * (motion_data1.DisplayTarget.x - motion_data2.DisplayStart.x) + motion_data1.DisplayTarget.y - b1;
//Debug.Log ("调试");
}
}
}
/// <summary>
/// constructor for Engine
/// </summary>
/// <param name="engineType">
/// select robot to drive;
/// select arm to move arm
/// </param>
/// <exception cref="ArgumentOutOfRangeException">thrown, if the engine type could not found</exception>
/// <exception cref="Exception">thrown, when a process in constructor fails</exception>
public Engine(EngineType engineType)
{
try
{
switch (engineType)
{
case EngineType.Robot:
//connect to motor to drive
_connector = new DeviceManager("EPOS2", "MAXON SERIAL V2", "USB", "USB0");
//motor to drive needs acceleration and deceleration
ProfileAcceleration = 3000;
ProfileDeceleration = 3000;
break;
case EngineType.Arm:
//connect to motor to move arm
_connector = new DeviceManager("EPOS", "MAXON_RS232", "RS232", "COM4");
//motor to move arm does not need acceleration and deceleration
ProfileAcceleration = 0;
ProfileDeceleration = 0;
break;
default:
throw new ArgumentOutOfRangeException(nameof(engineType), engineType, null);
}
//get baud rate info
var b = _connector.Baudrate;
//set connection properties
_connector.Baudrate = b;
_connector.Timeout = 500;
_epos1 = _connector.CreateDevice(Convert.ToUInt16(1));
_epos2 = _connector.CreateDevice(Convert.ToUInt16(2));
//ProfilePositionMode and ProfileVelocityMode are assigned to objects from the library
_ppm1 = _epos1.Operation.ProfilePositionMode;
_ppm2 = _epos2.Operation.ProfilePositionMode;
_pvm1 = _epos1.Operation.ProfileVelocityMode;
_pvm2 = _epos2.Operation.ProfileVelocityMode;
//StateMachines are assigned to objects from the library
_sm1 = _epos1.Operation.StateMachine;
_sm2 = _epos2.Operation.StateMachine;
//MotionInfo is assigned to an object from the library
_mi1 = _epos1.Operation.MotionInfo;
_mi2 = _epos2.Operation.MotionInfo;
//motors may be disabled
Enable();
//motors should initialize in VelocityMode
ActivateVelocityMode();
}
//an error may occur
catch (Exception)
{
ConsoleFormatter.Error("Failed to connect to the engine.",
"Type: " + engineType,
"Help: Check if the cable is plugged in properly.");
throw;
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2)
{
float k1;
float k2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
motion_data_circle.Center_Point = motion_data1.DisplayTarget; //保证圆心的坐标未改变
//k1,k2都不存在的时候，两者的方向矢量方向相反
if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x && motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x)
{
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y && motion_data2.DisplayTarget.y < motion_data2.DisplayStart.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
}
}
else if (motion_data1.DisplayStart.x == motion_data1.DisplayTarget.x && motion_data1.DisplayTarget.x != motion_data2.DisplayTarget.x)
{ //k1不存在,k2存在
k2 = (motion_data2.DisplayTarget.y - motion_data1.DisplayTarget.y) / (motion_data2.DisplayTarget.x - motion_data1.DisplayTarget.x);
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
}
}
else if (motion_data1.DisplayStart.x != motion_data1.DisplayTarget.x && motion_data1.DisplayTarget.x == motion_data2.DisplayTarget.x)
{ //k1存在，k2不存在
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
if (motion_data2.DisplayTarget.y > motion_data2.DisplayStart.y)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.LogError("测试");
} //修改过
}
else
{ //k1,k2都存在
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
k2 = (motion_data2.DisplayTarget.y - motion_data1.DisplayTarget.y) / (motion_data2.DisplayTarget.x - motion_data1.DisplayTarget.x);
int Left_Right_Array;
//标志左右象限
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
{
Left_Right_Array = 1;
}
else
{
Left_Right_Array = -1;
}
if (k1 == 0)
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * Left_Right_Array;
//Debug.Log ("调试");
if (k2 > 0)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
//"调试");
}
}
else if (k1 > 0)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 * Left_Right_Array * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * Left_Right_Array / Mathf.Sqrt(1 + k1 * k1);
if (k1 < k2)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
else if (k1 < 0)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 * Left_Right_Array * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * Left_Right_Array / Mathf.Sqrt(1 + k1 * k1);
if (k1 > k2)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2) //圆弧直线G03G01内角
{
float k1;
float k2;
float Up_Down = 0;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
if (motion_data1.DisplayTarget.y == motion_data1.Center_Point.y)
{
if (motion_data1.DisplayTarget.x > motion_data1.Center_Point.x)
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius2;
motion_data_circle.DisplayTarget.y = motion_data1.DisplayTarget.y;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data1.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data1.Center_Point.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
else
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.DisplayTarget.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius2;
motion_data_circle.DisplayTarget.y = motion_data1.DisplayTarget.y;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data1.Center_Point.y) / (motion_data2.DisplayStart.x - motion_data1.Center_Point.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
}
else
{
if (motion_data1.DisplayTarget.y > motion_data1.Center_Point.y)
{
Up_Down = 1;
}
else if (motion_data1.DisplayTarget.y < motion_data1.Center_Point.y)
{
Up_Down = -1;
}
if (motion_data1.DisplayTarget.x == motion_data1.Center_Point.x) //第一段终点和圆心所在的直线的斜率不存在
{
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * Up_Down;
if (motion_data2.DisplayStart.y == motion_data2.DisplayTarget.y)
{
motion_data_circle.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data_circle.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius2 * Up_Down;
Debug.Log("调试");
}
else
{
if (motion_data2.DisplayStart.x == motion_data2.DisplayTarget.x) //第二段直线的斜率不存在
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Up_Down;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
//Debug.Log ("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
if (motion_data2.DisplayStart.x > motion_data2.DisplayTarget.x) //第二段直线垂直直线的右侧
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
}
}
else if (motion_data2.DisplayStart.x > motion_data1.Center_Point.x) //第一段圆弧终点与圆心所在的垂直直线的右侧
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y) / (motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Up_Down;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
if (motion_data2.DisplayTarget.x < motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
}
else
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.Center_Point.y) / (motion_data1.DisplayTarget.x - motion_data1.Center_Point.x);
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.Center_Point.x == motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Up_Down;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y;
Debug.Log("调试");
}
else
{
k2 = (motion_data2.DisplayTarget.y - motion_data2.DisplayStart.y) / (motion_data2.DisplayTarget.x - motion_data2.DisplayStart.x);
if (motion_data2.DisplayTarget.x < motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
}
}
/*
* private void PreciseShootTarget()
* {
* MotionInfo targetMotion = new MotionInfo(MotionType.NavMeshAgent, m_Target.Position, m_Target.Velocity, m_Target.gameObject);
*
* Vector3 bulletDir = targetMotion.bounds.center - tankPlayer.shotPos.position;
* MotionInfo myBulletMotion = new MotionInfo(MotionType.Linear, tankPlayer.shotPos.position, bulletDir.normalized * m_BulletSpeed, Vector3.one * (m_BulletRadius * 2f));
*
* //initial guess
* float step;
* Bounds targetBounds = targetMotion.bounds;
* Bounds bulletBounds;
*
* int tolerance = 0;
* bool success = false;
* while (tolerance < 50)
* {
* step = Vector3.Distance(targetBounds.center, tankPlayer.shotPos.position) / (m_BulletSpeed * Time.fixedDeltaTime);
* targetBounds = targetMotion.PredictPosition(Mathf.CeilToInt(step));
*
* myBulletMotion.velocity = (targetBounds.center - tankPlayer.shotPos.position).normalized * m_BulletSpeed;
* bulletBounds = myBulletMotion.PredictPosition(Mathf.CeilToInt(step));
* tolerance++;
*
* if (targetBounds.Intersects(bulletBounds))
* {
* success = true;
* break;
* }
* }
*
* if(!success)
* {
* //Debug.DrawLine(tankPlayer.shotPos.position, targetBounds.center, Color.red);
* }
* else
* {
* Debug.DrawLine(tankPlayer.shotPos.position, targetBounds.center, Color.green);
* tankPlayer.AimAndShoot(targetBounds.center);
* }
* }
*/
//target selection:
//degree of threat:
//1. about to collide
//2. closest
//3. low health
//always choose the enemy with the least amount of health + sheild
private void SelectTarget(bool seek)
{
if (seek)
{
enemys = CheckSurroundingObjectsByType(m_PlayerSensorRadius,
(BasePlayer player) => player.teamIndex != tankPlayer.teamIndex && player.IsAlive);
}
else
{
enemys = CheckSurroundingObjectsByType(m_PlayerSensorRadius,
(BasePlayer player) => player.teamIndex != tankPlayer.teamIndex && player.IsAlive && !CheckObstaclesBetween(player.Position, tankPlayer.Position, 0f));
}
if (enemys.Length > 0)
{
int minHealth = NumOfHits(enemys[0].health, enemys[0].shield);
int minHealthIndex = 0;
int minColTime = int.MaxValue;
int minColTimeIndex = -1;
float minDist = float.MaxValue;
int minDistIndex = -1;
for (int i = 0; i < enemys.Length; i++)
{
int health = NumOfHits(enemys[i].health, enemys[i].shield);
if (minHealth > health)
{
minHealth = health;
minHealthIndex = i;
}
MotionInfo enemyMotion = new MotionInfo(MotionType.NavMeshAgent, enemys[i].Position, enemys[i].Velocity, enemys[i].gameObject);
int colTime = WillCollide(enemyMotion, m_PlayerMotionInfo, m_PredictionStep);
if (colTime != -1)
{
if (minColTime > colTime)
{
minColTime = colTime;
minColTimeIndex = i;
}
}
float dist = Vector3.Distance(tankPlayer.Position, enemys[i].Position);
if (dist < minDist)
{
minDist = dist;
minDistIndex = i;
}
}
//someone is about to run into us
if (minColTimeIndex != -1)
{
m_Target = enemys[minColTimeIndex];
}
else
{
if (minDistIndex != -1 && minDist < m_CombatDist + 5f)
{
m_Target = enemys[minDistIndex];
}
else
{
if (seek)
{
m_Target = enemys[minHealthIndex];
}
else
{
m_Target = null;
}
}
}
}
else
{
m_Target = null;
}
}
public void main(ref MotionInfo motion_data1, ref MotionInfo motion_data_circle, ref MotionInfo motion_data2)
{
float k1;
float k2;
float Radius1;
float Radius2;
//获取半径
if (motion_data1.D_Value == 0)
{
Radius1 = 0;
}
else
{
Radius1 = LoadRadiusValue.D_Value(motion_data1.D_Value);
}
if (motion_data2.D_Value == 0)
{
Radius2 = 0;
}
else
{
Radius2 = LoadRadiusValue.D_Value(motion_data2.D_Value);
}
Radius1 = Mathf.Abs(Radius1); //确保都是正值
Radius2 = Mathf.Abs(Radius2);
if (motion_data1.DisplayTarget.x == motion_data1.DisplayStart.x) //第一段直线的k1不存在
{
if (motion_data1.DisplayTarget.y > motion_data1.DisplayStart.y) //第一段直线的矢量方向向上
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
}
else //第一段直线的矢量方向垂直向下
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1;
// motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y;
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
}
else //第一段直线k1存在
{
k1 = (motion_data1.DisplayTarget.y - motion_data1.DisplayStart.y) / (motion_data1.DisplayTarget.x - motion_data1.DisplayStart.x);
if (k1 == 0)
{
int Left_Right_Array;
if (motion_data1.DisplayTarget.x > motion_data1.DisplayStart.x)
{
Left_Right_Array = 1;
}
else
{
Left_Right_Array = -1;
}
// motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x;
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 * Left_Right_Array;
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
if (motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2;
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2;
//Debug.Log ("调试");
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 * Left_Right_Array / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * Left_Right_Array * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试1");
}
}
else if (motion_data2.DisplayStart.x > motion_data1.DisplayStart.x) //右侧象限
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x - Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y + Radius1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
if (motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2; //不具有镜像特征
//Debug.Log ("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2; //不具有镜像特征
Debug.Log("调试");
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试1");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
}
}
else //左侧象限
{
motion_data1.DisplayTarget.x = motion_data1.DisplayTarget.x + Radius1 * k1 / Mathf.Sqrt(1 + k1 * k1);
motion_data1.DisplayTarget.y = motion_data1.DisplayTarget.y - Radius1 / Mathf.Sqrt(1 + k1 * k1);
if (motion_data2.DisplayStart.x == motion_data2.Center_Point.x)
{
if (motion_data2.Center_Point.y > motion_data2.DisplayStart.y)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2; //不具有镜像特征
Debug.Log("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x;
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2; //不具有镜像特征
Debug.Log("调试");
}
}
else
{
k2 = (motion_data2.Center_Point.y - motion_data2.DisplayStart.y) / (motion_data2.Center_Point.x - motion_data2.DisplayStart.x);
if (motion_data2.Center_Point.x > motion_data2.DisplayStart.x)
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x + Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y + Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
Debug.Log("调试");
}
else
{
motion_data_circle.DisplayTarget.x = motion_data2.DisplayStart.x - Radius2 / Mathf.Sqrt(1 + k2 * k2);
motion_data_circle.DisplayTarget.y = motion_data2.DisplayStart.y - Radius2 * k2 / Mathf.Sqrt(1 + k2 * k2);
//Debug.Log ("调试");
}
}
}
}
}