public Unit(string spritecode, Vector2 pos)
{
kinematics = new Kinematics();
kinematics.pos = pos;
kinematics.g = 1;
TimeManager.Entities.Add(this);
sprite = Resources.Load <Sprite>(Filepaths.images[spritecode]);
spriteOrder = 0;
PrefabCreator.CreateSprite(this);
colliderwidthcs = 1.2f;
colliderheightcs = 0.8f;
PrefabCreator.CreateCollider(this);
team = Team.wildlifegeneric;
health = 100;
//initialize basic skill
jump = new Jump(this);
walkleft = new WalkLeft(this);
walkright = new WalkRight(this);
walkstop = new WalkStop(this);
attack = new Shoot(this); // 2 s cooldown
}
//构型切换事件
private void ChangePose(int value)
{
Vector3 Postion;
Vector3 Pose;
//运动学正解计算出姿态
Kinematics.forwardKinematics(axisManage.Axis1.GetAngle(),
axisManage.Axis2.GetAngle(),
axisManage.Axis3.GetAngle(),
axisManage.Axis4.GetAngle(),
axisManage.Axis5.GetAngle(),
axisManage.Axis6.GetAngle(),
out Postion,
out Pose
);
//运动学反解计算出8组解
var kk = Kinematics.InverseKinematics(Postion, Pose);
//判断角度是否有效
if (kk[value, 0] == kk[value, 0] &&
kk[value, 1] == kk[value, 1] &&
kk[value, 2] == kk[value, 2] &&
kk[value, 3] == kk[value, 3] &&
kk[value, 4] == kk[value, 4] &&
kk[value, 5] == kk[value, 5]
)
{
axisManage.SetJPos((float)kk[value, 0], (float)kk[value, 1], (float)kk[value, 2], (float)kk[value, 3], (float)kk[value, 4], (float)kk[value, 5]);
}
else
{
Debug.LogError("当前构型不存在");
}
}
public override SteeringOutput GetSteering()
{
direction = target.transform.position - character.transform.position;
distance = direction.magnitude;
float speed = character.linearVel.magnitude;
if (speed <= distance / maxPrediction)
{
prediction = maxPrediction;
}
else
{
prediction = distance / speed;
}
//Debug.Log(speed);
//Debug.Log(prediction);
Kinematics myTarget = target.GetComponent(typeof(Kinematics)) as Kinematics;
if (myTarget == null)
{
return(base.GetSteering());
}
else
{
target.transform.position += myTarget.linearVel * prediction;
return(base.GetSteering());
}
}
public TestWindow()
{
InitializeComponent();
kinematics = new Kinematics();
imageProcessing = new ImageProcessing(pictureBox1);
gameLogics = new GameLogics(kinematics, imageProcessing);
}
// Update is called once per frame
public void setUp(Vector3 _target)
{
//canvas.enabled = false;
target = _target;
if (leader)
{
path = GameObject.Find("PathManager").GetComponent <PathManager>();
}
//debug = Instantiate(target);
kinematics = this.GetComponent <Kinematics>();
kinematics.character = this.gameObject;
kinematics.maxSpeed = maxSpeed;
kinematics.target = _target;
character = this.GetComponent <Character>();
character.r = rotation;
character.a = angular;
character.l = linear;
collision = this.GetComponent <Collision>();
collision.character = character;
collision.lookAhead = lookAhead;
collision.avoidDistance = avoidDistance;
collision.whiskerLookAhead = lookAhead / 2;
}
public void AddComponent(Kinematics key, double[] series)
{
if (!components.ContainsKey(key))
{
components.Add(key, new double[Length]);
}
components[key] = series;
}
public void Kinematics_Drive_Forward()
{
yomo.Utility.Settings.Testing = true;
var k = new Kinematics();
k.Drive(50, 50);
Assert.Equal(50, k.left.Speed);
Assert.Equal(50, k.right.Speed);
}
private void Update()
{
Kinematics.EvaluateInverse(_target.position, _jointsWithRotations, _learningRate, _distanceThreshold);
foreach (var jointWithRotation in _jointsWithRotations)
{
var(joint, angle) = jointWithRotation;
joint.transform.localRotation = Quaternion.AngleAxis(angle, joint.Axis);
}
}
public void Kinematics_CalculateSpeed_ALotSlower()
{
yomo.Utility.Settings.Testing = true;
var k = new Kinematics();
TimeSpan ts = new TimeSpan(10);
var newSpeed = k.CalculateSpeed(50, 75, ts);
Assert.True(newSpeed < 75);
}
public void Kinematics_CalculateSpeed_ALittleFaster()
{
yomo.Utility.Settings.Testing = true;
var k = new Kinematics();
TimeSpan ts = new TimeSpan(10);
var newSpeed = k.CalculateSpeed(50, 49, ts);
Assert.True(newSpeed > 49);
}
// Start is called before the first frame update
void Start()
{
animator = GetComponent <Animator>();
attack = GetComponent <Attack>();
kinematics = GetComponent <Kinematics>();
kinematics.SetTarget(target.transform);
kinematics.SetState(KinematicStates.Wander);
isMoving = true;
}
public void Kinematics_CalculateSpeed_Faster_WarmedUp()
{
yomo.Utility.Settings.Testing = true;
var k = new Kinematics();
TimeSpan ts = new TimeSpan(10);
var newSpeed = k.CalculateSpeed(50, 25, ts);
newSpeed = k.CalculateSpeed(50, newSpeed, ts);
Assert.True(newSpeed > 25);
}
/// <summary>
/// Time series.
/// </summary>
public double[] this[Kinematics k]
{
get
{
if (!components.ContainsKey(k))
{
throw new InvalidOperationException();
}
return(components[k]);
}
}
public void KineTestMethod()
{
Kine3 toTest = new Kine3();
toTest.InitXvelo = 0f;
toTest.Xacc = 0f;
toTest.Xdistance = 0f;
toTest = Kinematics.GetFinalVelo(toTest);
Assert.AreEqual(true, toTest.FinalXvelo.HasValue);
Assert.AreEqual(0f, toTest.FinalXvelo);
toTest.InitXvelo = 1f;
toTest.FinalXvelo = null;
toTest = Kinematics.GetFinalVelo(toTest);
Assert.AreEqual(true, toTest.FinalXvelo.HasValue);
Assert.AreEqual(1f, toTest.FinalXvelo);
}
private float[][] GenerateVelocityData(
double v0 = 0, double a = 0, double d = 0, double t = 5, double dt = 1.0 / 60)
{
int frames = (int)(t / dt) + 1;
float[] ts = new float[frames];
float[] vs = new float[frames];
ts[0] = 0;
vs[0] = (float)v0;
for (int i = 1; i < frames; i++)
{
ts[i] = (float)(i * dt);
vs[i] = (float)Kinematics.Velocity(ts[i], v0, a, d);
}
return(new float[][] { ts, vs });
}
public TestForm()
{
InitializeComponent();
gameLogics = new GameLogics(Piece.O);
kinematics = new Kinematics();
imageProcessor = new Camera();
// setting up the communication between the modules:
gameLogics.PostMessageShowRequest += PostMessageHandler; // game logics handles its own post messages
kinematics.PostMessageShowRequest += PostMessageHandler; // game logics handles the post messages of the kinematics
imageProcessor.PostMessageShowRequest += PostMessageHandler; // game logics handles the post messages of the image processing
kinematics.RobotStatusChanged += gameLogics.RobotStatusChangedHandler; // game logics handles the changes of the robot arm status (source: kinematics)
gameLogics.RobotMovementReqest += kinematics.RobotMovementRequestHandler; // kinematics handles the robot movement requests (source: game logics)
imageProcessor.TableStateChanged += gameLogics.TableSetupChangedHandler;
imageProcessor.GameStatusChanged += new EventHandler <InterfaceModule.GameStatusChangedEventArgs>(cam1_CameraStatusChanged);
imageProcessor.NextPieceChanged += gameLogics.NextPieceChangedHandler;
imageProcessor.Init(pictureBox1, pictureBox2, pictureBox3, 17, 452, 200, 500, 359, 10, "Consolas");
}
private void UpdateCPos()
{
Vector3 Pos;
Vector3 pose;
double q1 = axisManage.Axis1.GetAngle();
double q2 = axisManage.Axis2.GetAngle();
double q3 = axisManage.Axis3.GetAngle();
double q4 = axisManage.Axis4.GetAngle();
double q5 = axisManage.Axis5.GetAngle();
double q6 = axisManage.Axis6.GetAngle();
Kinematics.forwardKinematics(q1, q2, q3, q4, q5, q6, out Pos, out pose);
txt_Cpos[0].text = "X:" + (Pos.x).ToString("0.0000") + "mm";
txt_Cpos[1].text = "Y:" + (Pos.y).ToString("0.0000") + "mm";
txt_Cpos[2].text = "Z:" + (Pos.z).ToString("0.0000") + "mm";
txt_Cpos[3].text = "RX:" + (pose.x).ToString("0.0000") + "°";
txt_Cpos[4].text = "RY:" + (pose.y).ToString("0.0000") + "°";
txt_Cpos[5].text = "RZ:" + (pose.z).ToString("0.0000") + "°";
}
private void button1_Click(object sender, EventArgs e)
{
DataReady = false;
Kinematics k = new Kinematics();
dataGridView1.DataSource = null;
Force PIDCorrection = new Force();
k.Forces.Add(PIDCorrection);
PIDLoop pid = new PIDLoop(k);
Force Disturbance = new Force();
k.Forces.Add(Disturbance);
k.mass = 1;
Data.Clear();
Double RunningTime = 0;
k.XPosition = 0;
k.XVelocity = -20;
for (int i = 0; i < 10000; i++)//sim loop
{
RunningTime+= k.time;
if (i > 50 && i< 3000)
{
Disturbance.X = -100;
PIDCorrection.X = pid.calculateCorrection(50);
}
else if (i >= 3000)
{
PIDCorrection.X = pid.calculateCorrection(25);
}
k.CalculateKinematics();
DataRow newRow = Data.NewRow();
newRow["time"] = RunningTime;
newRow["position"] = k.XPosition;
Data.Rows.Add(newRow);
}
DataReady = true;
dataGridView1.DataSource = Data;
this.Invalidate();
}
//Sets ballistics info
void AcquireBallisticData(Vector3 position)
{
float displacement;
float flightTime;
float shotAngle;
float initialSpeed;
displacement = Vector3.Magnitude(Common.RemoveYComp(position - transform.position));
float distRatio = Common.GetRatio(displacement, 0.5f, range);
//angle determined by the distance to target
shotAngle = Mathf.Lerp(Kinematics.min_angle, Kinematics.max_angle, distRatio);
flightTime = Kinematics.GetFlightTimeFromDist(Kinematics.gravity, displacement, shotAngle, activeMuzzle.position.y - position.y);
initialSpeed = Kinematics.GetInitialSpeed(displacement, shotAngle, flightTime);
ballistics.angle = shotAngle;
ballistics.distance = displacement;
ballistics.initialSpeed = initialSpeed;
ballistics.time = flightTime;
}
// Use this for initialization
void Start()
{
scenario = 0;
prevScenario = -1;
// UDP Send Port
sendSocket = new Socket(AddressFamily.InterNetwork, SocketType.Dgram, ProtocolType.Udp);
IPAddress sendTo = IPAddress.Parse("127.0.0.1");
sendEndPoint = new IPEndPoint(sendTo, sport);
sendSocket.SendBufferSize = 0;
// UDP Receive Port
inremoteEndPoint = new IPEndPoint(IPAddress.Any, rport);
inclient = new UdpClient(rport);
inclient.Client.ReceiveBufferSize = 8;
propellers = transform.GetComponent <AudioThrust> ();
movement = transform.GetComponent <Kinematics> ();
mainCamera = GameObject.Find("Camera");
}
/// <summary>
/// The main form of the program.
/// </summary>
public RoboTicTacToe()
{
InitializeComponent();
// creating the components:
gameLogics = new GameLogics(Piece.O);
kinematics = new Kinematics();
imageProcessor = new Camera();
// establishing communication:
gameLogics.PostMessageShowRequest += PostMessageHandler;
kinematics.PostMessageShowRequest += PostMessageHandler;
imageProcessor.PostMessageShowRequest += PostMessageHandler;
kinematics.RobotStatusChanged += gameLogics.RobotStatusChangedHandler; // game logics handles the changes of the robot arm status (source: kinematics)
gameLogics.RobotMovementReqest += kinematics.RobotMovementRequestHandler; // kinematics handles the robot movement requests (source: game logics)
imageProcessor.TableStateChanged += gameLogics.TableSetupChangedHandler;
imageProcessor.NextPieceChanged += gameLogics.NextPieceChangedHandler;
imageProcessor.GameStatusChanged += new EventHandler <InterfaceModule.GameStatusChangedEventArgs>(gameStatusChanged);
// initialisation:
imageProcessor.Init(pictureBox1, pictureBox2, pictureBox3, 17, 452, 200, 500, 359, 10, "Consolas");
}
private void XYZMove(float dx, float dy, float dz)
{
Vector3 Position;
Vector3 pose;
double q1 = Axis1.GetAngle();
double q2 = Axis2.GetAngle();
double q3 = Axis3.GetAngle();
double q4 = Axis4.GetAngle();
double q5 = Axis5.GetAngle();
double q6 = Axis6.GetAngle();
Kinematics.forwardKinematics(q1, q2, q3, q4, q5, q6, out Position, out pose);
Position.x = Position.x + dx;
Position.y = Position.y + dy;
Position.z = Position.z + dz;
var kk = Kinematics.InverseKinematics(Position, pose);
for (int i = 0; i < 6; i++)
{
if (kk[i, 0] == kk[i, 0] &&
kk[i, 1] == kk[i, 1] &&
kk[i, 2] == kk[i, 2] &&
kk[i, 3] == kk[i, 3] &&
kk[i, 4] == kk[i, 4] &&
kk[i, 5] == kk[i, 5]
)
{
SetJPos(kk[i, 0], kk[i, 1], kk[i, 2], kk[i, 3], kk[i, 4], kk[i, 5]);
GameObject obj1 = GameObject.CreatePrimitive(PrimitiveType.Sphere);
obj1.GetComponent <MeshRenderer>().material = Ballmaterial;
//设置物体的位置Vector3三个参数分别代表x,y,z的坐标数
obj1.transform.position = rbt_tool.transform.position;
obj1.transform.localScale = new Vector3(0.01f, 0.01f, 0.01f);
obj1.transform.parent = Ball.transform;
break;
}
}
}
//沿世界坐标系移动到目标位置 PtP移动
public void CMove(CPostion pose, bool CreateBall = false)
{
Vector3 Axis1_3target = new Vector3();
Vector3 Axis4_6target = new Vector3();
//运动学正解计算出8组解
var kk = Kinematics.InverseKinematics(pose.Postion, pose.Pose);
for (int i = 0; i < 8; i++)
{
//判断角度是否有效
if (kk[i, 0] == kk[i, 0] &&
kk[i, 1] == kk[i, 1] &&
kk[i, 2] == kk[i, 2] &&
kk[i, 3] == kk[i, 3] &&
kk[i, 4] == kk[i, 4] &&
kk[i, 5] == kk[i, 5]
)
{
Axis1_3target = new Vector3((float)kk[i, 0], (float)kk[i, 1], (float)kk[i, 2]);
Axis4_6target = new Vector3((float)kk[i, 3], (float)kk[i, 4], (float)kk[i, 5]);
JPostion jp = new JPostion(Axis1_3target, Axis4_6target);
jp.CreateBall = CreateBall;
lock (PosList)
{
PosList.Enqueue(jp);
}
break;
}
}
Axis1_3Visual.x = Axis1_3target.x;
Axis1_3Visual.y = Axis1_3target.y;
Axis1_3Visual.z = Axis1_3target.z;
Axis4_6Visual.x = Axis4_6target.x;
Axis4_6Visual.y = Axis4_6target.y;
Axis4_6Visual.z = Axis4_6target.z;
}
private PlotModel CreatePlot(TimeSeriesPlotData xSeries, TimeSeriesPlotData ySeries, Kinematics component, string abbreviation, string title)
{
if (xSeries == null || ySeries == null || xSeries == ySeries)
{
return(null);
}
PlotModel model = new PlotModel();
model.PlotType = PlotType.XY;
model.Title = string.Format("{0} v {1} - {2}", xSeries.Label, ySeries.Label, title);
LinearAxis xAxis = new LinearAxis();
xAxis.Position = AxisPosition.Bottom;
xAxis.MajorGridlineStyle = LineStyle.Solid;
xAxis.MinorGridlineStyle = LineStyle.Dot;
xAxis.MinimumPadding = 0.05;
xAxis.MaximumPadding = 0.1;
xAxis.Title = string.Format("{0} {1} ({2})", xSeries.Label, title, abbreviation);
model.Axes.Add(xAxis);
LinearAxis yAxis = new LinearAxis();
yAxis.Position = AxisPosition.Left;
yAxis.MajorGridlineStyle = LineStyle.Solid;
yAxis.MinorGridlineStyle = LineStyle.Dot;
yAxis.MinimumPadding = 0.05;
yAxis.MaximumPadding = 0.1;
yAxis.Title = string.Format("{0} {1} ({2})", ySeries.Label, title, abbreviation);
model.Axes.Add(yAxis);
LineSeries series = new LineSeries();
//series.Title =
//series.Color = OxyColor.;
series.MarkerType = MarkerType.Circle;
series.Smooth = true;
double[] xPoints = xSeries.TimeSeriesCollection[component];
double[] yPoints = ySeries.TimeSeriesCollection[component];
long[] xTimes = xSeries.TimeSeriesCollection.Times;
long[] yTimes = ySeries.TimeSeriesCollection.Times;
// The plot is only defined in the range of common time coordinates.
int xIndex = 0;
int yIndex = 0;
while (xIndex < xTimes.Length && yIndex < yTimes.Length)
{
long xTime = xTimes[xIndex];
long yTime = yTimes[yIndex];
if (xTime < yTime)
{
xIndex++;
continue;
}
else if (yTime < xTime)
{
yIndex++;
continue;
}
else
{
series.Points.Add(new DataPoint(xPoints[xIndex], yPoints[yIndex]));
xIndex++;
yIndex++;
}
}
model.Series.Add(series);
return(model);
}
public PIDLoop(Kinematics k)
{
this.k = k;
}
// Update is called once per frame
void Update()
{
if (float.IsNaN(angularVel))
{
angularVel = 0.0f;
}
SteeringOutput moving = new SteeringOutput();
SteeringOutput turning = new SteeringOutput();
seeking.character = this;
seeking.target = aiTarget;
SteeringOutput movement = new SteeringOutput();
switch (movementType)
{
case "Seek":
moving = seeking.GetSteering();
if (moving == null)
{
movement.linear = Vector3.zero;
}
else
{
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
case "Flee":
flee = true;
moving = seeking.GetSteering();
if (moving == null)
{
movement.linear = Vector3.zero;
}
else
{
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
case "Arrive":
moving = arrive.GetSteering();
if (moving == null)
{
movement.linear = Vector3.zero;
}
else
{
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
case "Face":
facing.target = aiTarget;
facing.character = this;
//moving = seeking.GetSteering();
turning = facing.GetSteering();
movement.linear = Vector3.zero;
movement.angular = turning.angular;
break;
case "Seek Align":
aligned.character = this;
aligned.target = aiTarget;
moving = seeking.GetSteering();
turning = aligned.GetSteering();
movement.linear = moving.linear;
movement.angular = turning.angular;
break;
case "Seek Face":
facing.character = this;
facing.target = aiTarget;
moving = seeking.GetSteering();
turning = facing.GetSteering();
if (moving == null)
{
movement.linear = Vector3.zero;
}
else
{
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
case "Seek Look":
looky.character = this;
looky.target = aiTarget;
//Debug.Log(looky.GetSteering());
if (looky.GetSteering() == null)
{
moving = seeking.GetSteering();
movement.linear = moving.linear;
movement.angular = 0;
}
else
{
moving = seeking.GetSteering();
turning = looky.GetSteering();
//Debug.Log(looky.GetSteering());
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
case "Arrive Align":
aligned.character = this;
aligned.target = aiTarget;
moving = arrive.GetSteering();
turning = aligned.GetSteering();
if (moving == null)
{
movement.linear = Vector3.zero;
}
else
{
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
case "Arrive Face":
facing.character = this;
facing.target = aiTarget;
moving = arrive.GetSteering();
turning = facing.GetSteering();
movement.linear = moving.linear;
movement.angular = turning.angular;
break;
case "Arrive Look":
looky.character = this;
//looky.target = aiTarget;
arrive.character = this;
arrive.target = aiTarget;
moving = arrive.GetSteering();
turning = looky.GetSteering();
movement.linear = moving.linear;
movement.angular = turning.angular;
Debug.Log(movement.angular);
break;
case "Path Follow":
pathFollow.path = aiTargets;
pathFollow.character = this;
pathFollow.maxAccel = maxAccel;
pathFollow.maxSpeed = maxSpeed;
pathFollow.targetRad = targetRad;
pathFollow.slowRad = slowRad;
moving = pathFollow.GetSteering();
movement.linear = moving.linear;
break;
case "Path Follow Face":
pathFollow.path = aiTargets;
pathFollow.character = this;
pathFollow.maxAccel = maxAccel;
pathFollow.maxSpeed = maxSpeed;
pathFollow.targetRad = targetRad;
pathFollow.slowRad = slowRad;
moving = pathFollow.GetSteering();
aiTarget = pathFollow.target;
facing.character = this;
facing.target = aiTarget;
turning = facing.GetSteering();
movement.linear = moving.linear;
movement.angular = turning.angular;
break;
case "Separate":
separate.character = this;
separate.avoid = kinTargets;
separate.threshold = threshold;
separate.decay = decay;
//separate.avoid = avoidance;
moving = separate.GetSteering();
movement.linear = moving.linear;
movement.angular = 0;
break;
case "Pursue":
pursuing.character = this;
pursuing.target = aiTarget;
moving = pursuing.GetSteering();
movement.linear = moving.linear;
movement.angular = 0;
break;
case "Avoidance":
avoid.character = this;
avoid.targets = kinTargets;
kinTarget = avoid.firstTarget;
moving = avoid.GetSteering();
movement.linear = moving.linear;
movement.angular = 0;
break;
case "Obstacles":
obstacles.character = this;
//obstacles.target = aiTarget;
looky.character = this;
looky.target = obstacles.target;
moving = obstacles.GetSteering();
turning = looky.GetSteering();
movement.linear = moving.linear;
movement.angular = turning.angular;
break;
case "Flocking":
arrive.character = this;
arrive.target = aiTarget;
arrive.maxAccel = maxAccel;
arrive.maxSpeed = maxSpeed;
arrive.targetRad = targetRad;
arrive.slowRad = slowRad;
looky.character = this;
obstacles.character = this;
separate.character = this;
separate.avoid = kinTargets;
separate.threshold = threshold;
separate.decay = decay;
behaviors[0] = new BehaviorAndWeight();
behaviors[0].behavior = arrive;
behaviors[0].weight = 35f;
behaviors[1] = new BehaviorAndWeight();
behaviors[1].behavior = separate;
behaviors[1].weight = 1f;
behaviors[2] = new BehaviorAndWeight();
behaviors[2].behavior = looky;
behaviors[2].weight = 50f;
behaviors[3] = new BehaviorAndWeight();
behaviors[3].behavior = obstacles;
behaviors[3].weight = 10f;
flocking.behaviors = behaviors;
flocking.maxAccel = maxAccel;
flocking.maxRotation = maxRot;
moving = flocking.GetSteering();
movement.linear = moving.linear;
movement.angular = moving.angular;
break;
default:
facing.character = this;
facing.target = aiTarget;
moving = seeking.GetSteering();
turning = aligned.GetSteering();
if (moving == null)
{
movement.linear = Vector3.zero;
}
else
{
movement.linear = moving.linear;
movement.angular = turning.angular;
}
break;
}
// Update linear and angular velocities
//Debug.Log(movement.angular);
linearVel += movement.linear * Time.deltaTime;
angularVel += movement.angular * Time.deltaTime;
//angularInc *= Mathf.Rad2Deg;
transform.position += linearVel * Time.deltaTime * maxSpeed;
angularInc = new Vector3(0, angularVel * Time.deltaTime * angVelMax, 0);
//Debug.Log(angularInc)
if (!float.IsNaN(angularInc.y))
{
transform.eulerAngles += angularInc;
}
//Debug.Log(linearVel);
if (linearVel.magnitude > maxSpeed)
{
linearVel.Normalize();
linearVel *= maxSpeed;
}
}
private PlotModel CreatePlot(IEnumerable <TimeSeriesPlotData> timeSeriesPlotData, Kinematics component, string abbreviation, string title, TimeModel timeModel)
{
if (timeSeriesPlotData == null)
{
return(null);
}
PlotModel model = new PlotModel();
model.PlotType = PlotType.XY;
model.Title = title;
LinearAxis xAxis = new LinearAxis();
xAxis.Position = AxisPosition.Bottom;
xAxis.MajorGridlineStyle = LineStyle.Solid;
xAxis.MinorGridlineStyle = LineStyle.Dot;
xAxis.MinimumPadding = 0.02;
xAxis.MaximumPadding = 0.05;
model.Axes.Add(xAxis);
if (timeModel == TimeModel.Absolute || timeModel == TimeModel.Relative)
{
xAxis.Title = ScreenManagerLang.DataAnalysis_TimeAxisMilliseconds;
}
else
{
xAxis.Title = "Time";
}
LinearAxis yAxis = new LinearAxis();
yAxis.Position = AxisPosition.Left;
yAxis.MajorGridlineStyle = LineStyle.Solid;
yAxis.MinorGridlineStyle = LineStyle.Dot;
yAxis.MinimumPadding = 0.05;
yAxis.MaximumPadding = 0.1;
yAxis.Title = string.Format("{0} ({1})", title, abbreviation);
model.Axes.Add(yAxis);
foreach (TimeSeriesPlotData tspd in timeSeriesPlotData)
{
LineSeries series = new LineSeries();
series.Title = tspd.Label;
series.Color = OxyColor.FromArgb(tspd.Color.A, tspd.Color.R, tspd.Color.G, tspd.Color.B);
series.MarkerType = MarkerType.None;
series.Smooth = true;
double[] points = tspd.TimeSeriesCollection[component];
long[] times = tspd.TimeSeriesCollection.Times;
double firstTime = TimestampToMilliseconds(times[0]);
double timeSpan = TimestampToMilliseconds(times[times.Length - 1]) - firstTime;
for (int i = 0; i < points.Length; i++)
{
double value = points[i];
if (double.IsNaN(value))
{
continue;
}
double time = TimestampToMilliseconds(times[i]);
switch (timeModel)
{
case TimeModel.Relative:
time -= firstTime;
break;
case TimeModel.Normalized:
time = (time - firstTime) / timeSpan;
break;
case TimeModel.Absolute:
default:
break;
}
series.Points.Add(new DataPoint(time, value));
}
model.Series.Add(series);
}
return(model);
}
private void AddPlotSpecification(Kinematics component, string abbreviation, string label)
{
cmbPlotSpec.Items.Add(new TimeSeriesPlotSpecification(label, component, abbreviation));
}
// controls, Vector3D
// should keep controls between -1 and 1
public void go(Controls cont) {
if(!this.rotorShaft.IsConnected()) {
return;
}
bool topNull = false;
if(rotorShaftMonitor == null) {
topNull = this.rotorShaft.Top == null;
rotorShaftMonitor = (topNull ? null : new Kinematics(this.rotorShaft, this.rotorShaft.Top));
}
//check if direction hasa changed
bool positive_now = true;
if(!topNull) {
rotorShaftMonitor.Update(prog.Runtime.TimeSinceLastRun.TotalSeconds);
float RPM = (float)(rotorShaftMonitor.VelocityAngularCurrent * RADsToRPM).Y;
positive_now = RPM > 0;
}
if(positive_now != positiveDirection) {
foreach(Rotor blade in blades) {
// forward direction is reversed if the main rotor is in reverse
blade.setForwardDir(Vector3D.Cross(rotorShaft.WorldMatrix.Up, blade.theBlock.WorldMatrix.Up) * (rotorShaft.TargetVelocityRPM > 0 ? 1 : -1));
}
positiveDirection = positive_now;
}
if(printinfo) {
theStr = "";
}
if(cont.collective > maxValue) {
cont.collective = maxValue;
} else if(cont.collective < -maxValue) {
cont.collective = -maxValue;
}
if(cont.cyclicF > maxValue) {
cont.cyclicF = maxValue;
} else if(cont.cyclicF < -maxValue) {
cont.cyclicF = -maxValue;
}
if(cont.cyclicR > maxValue) {
cont.cyclicR = maxValue;
} else if(cont.cyclicR < -maxValue) {
cont.cyclicR = -maxValue;
}
if(printinfo) {
theStr += "collective: " + cont.collective;
theStr += "\ncyclicR: " + cont.cyclicR;
theStr += "\ncyclicF: " + cont.cyclicF;
}
bool first = true;
foreach(Rotor blade in blades) {
if(!blade.theBlock.IsConnected()) {
continue;
}
Vector3D bladeForward = blade.getForwardDir();
Vector3D vec =
// point it forward
bladeForward +
// collective TODO find out why collective needs to be reversed
rotorShaft.WorldMatrix.Up * cont.collective * -1 +
// cyclic right
Vector3D.Dot(bladeForward, rotorShaft.WorldMatrix.Right) * rotorShaft.WorldMatrix.Up * cont.cyclicR +
// cyclic forward
Vector3D.Dot(bladeForward, rotorShaft.WorldMatrix.Forward) * rotorShaft.WorldMatrix.Up * cont.cyclicF;
blade.setFromVec(vec);
if(printinfo && first) {
first = false;
theStr += "\nfirst basevec: " + blade.localForwardDir.ToString("0.0");
theStr += "\nfirst desired: " + vec.ToString("0.0");
}
}
}
//沿世界坐标系移动到目标位置 直线移动
public void CLine(CPostion mpose, bool CreateBall = false)
{
Vector3 Position;
Vector3 pose;
Vector3 Axis1_3target = new Vector3();
Vector3 Axis4_6target = new Vector3();
var kk = Kinematics.InverseKinematics(mpose.Postion, mpose.Pose);
//运动学正解计算出当前位置，
Kinematics.forwardKinematics(Axis1_3Visual.x, Axis1_3Visual.y, Axis1_3Visual.z, Axis4_6Visual.x, Axis4_6Visual.y, Axis4_6Visual.z, out Position, out pose);
//计算两者之间直线长度
double distance = mpose.GetDistance(new CPostion(Position, pose));
//计算插值数量
int count = (int)(distance / interLine);
//计算单个轴的增量
//如果距离很小
if (count == 0)
{
var ikine_t = Kinematics.InverseKinematics(mpose.Postion, mpose.Pose);
for (int i = 0; i < 8; i++)
{
//判断角度是否有效
if (ikine_t[i, 0] == ikine_t[i, 0] &&
ikine_t[i, 1] == ikine_t[i, 1] &&
ikine_t[i, 2] == ikine_t[i, 2] &&
ikine_t[i, 3] == ikine_t[i, 3] &&
ikine_t[i, 4] == ikine_t[i, 4] &&
ikine_t[i, 5] == ikine_t[i, 5]
)
{
Axis1_3target = new Vector3((float)ikine_t[i, 0], (float)ikine_t[i, 1], (float)ikine_t[i, 2]);
Axis4_6target = new Vector3((float)ikine_t[i, 3], (float)ikine_t[i, 4], (float)ikine_t[i, 5]);
JPostion jp = new JPostion(Axis1_3target, Axis4_6target);
jp.CreateBall = CreateBall;
lock (PosList)
{
PosList.Enqueue(jp);
}
break;
}
}
}
else
{
double dx = (mpose.x - Position.x) / count;
double dy = (mpose.y - Position.y) / count;
double dz = (mpose.z - Position.z) / count;
double drx = (mpose.rx - pose.x) / count;
double dry = (mpose.ry - pose.y) / count;
double drz = (mpose.rz - pose.z) / count;
//细分
for (int j = 0; j < count; j++)
{
Position.x += (float)dx;
Position.y += (float)dy;
Position.z += (float)dz;
pose.x += (float)drx;
pose.y += (float)dry;
pose.z += (float)drz;
//计算逆解
var ikine_t = Kinematics.InverseKinematics(Position, pose);
for (int i = 0; i < 8; i++)
{
//判断角度是否有效
if (ikine_t[i, 0] == ikine_t[i, 0] &&
ikine_t[i, 1] == ikine_t[i, 1] &&
ikine_t[i, 2] == ikine_t[i, 2] &&
ikine_t[i, 3] == ikine_t[i, 3] &&
ikine_t[i, 4] == ikine_t[i, 4] &&
ikine_t[i, 5] == ikine_t[i, 5]
)
{
Axis1_3target = new Vector3((float)ikine_t[i, 0], (float)ikine_t[i, 1], (float)ikine_t[i, 2]);
Axis4_6target = new Vector3((float)ikine_t[i, 3], (float)ikine_t[i, 4], (float)ikine_t[i, 5]);
JPostion jp = new JPostion(Axis1_3target, Axis4_6target);
jp.CreateBall = CreateBall;
lock (PosList)
{
PosList.Enqueue(jp);
}
break;
}
}
}
}
//更新当前虚拟坐标
Axis1_3Visual.x = Axis1_3target.x;
Axis1_3Visual.y = Axis1_3target.y;
Axis1_3Visual.z = Axis1_3target.z;
Axis4_6Visual.x = Axis4_6target.x;
Axis4_6Visual.y = Axis4_6target.y;
Axis4_6Visual.z = Axis4_6target.z;
}
public virtual SteeringOutput GetSteering()
{
shortestTime = float.PositiveInfinity;
//firstTarget = null;
foreach (Kinematics target in targets)
{
relativePos = target.transform.position - character.transform.position;
relativeVel = character.linearVel - target.linearVel;
relativeSpeed = relativeVel.magnitude;
timeToCollision = Vector3.Dot(relativePos, relativeVel) / (relativeSpeed * relativeSpeed);
distance = relativePos.magnitude;
minSeparation = distance - relativeSpeed * timeToCollision;
if (minSeparation > 2 * radius)
{
//Debug.Log("1");
continue;
}
else if (timeToCollision > 0 && timeToCollision < shortestTime)
{
shortestTime = timeToCollision;
firstTarget = target;
firstMinSeparation = minSeparation;
firstDist = distance;
firstRelativePos = relativePos;
firstRelativeVel = relativeVel;
//Debug.Log(2);
}
}
//if(firstMinSeparation <= 0 || firstDist < 2 * radius)
//{
// relativePos = firstTarget.transform.position - character.transform.position;
//}
//else
//{
// relativePos = firstRelativePos + firstRelativeVel * shortestTime;
//}
SteeringOutput result = new SteeringOutput();
if (!firstTarget)
{
Debug.Log("Oops");
result.linear = Vector3.zero;
result.angular = 0;
return(result);
}
float dotResult = Vector3.Dot(character.linearVel.normalized, firstTarget.linearVel.normalized);
if (dotResult < -0.9 && dotResult > -1.1)
{
result.linear = -firstTarget.transform.right;
}
else
{
result.linear = -firstTarget.linearVel;
}
//-target.linearVel;
relativePos.Normalize();
//result.linear = relativePos * maxAccel;
result.angular = 0;
return(result);
}
public TimeSeriesPlotSpecification(string label, Kinematics component, string abbreviation)
{
this.Label = label;
this.Component = component;
this.Abbreviation = abbreviation;
}
