public void SetsPoseShortcut()
{
Actor a = new Actor();
Pose p = new Pose();
a.Add(p);
Assert.AreSame(p, a.Pose);
}
public BrownOut(ContentManager contentManager)
{
Pose = new Pose(Matrix33F.CreateRotationX(-ConstantsF.PiOver2));
// Smoke on a ring.
var outerRingSmoke = CreateSmoke(contentManager);
outerRingSmoke.Effectors.Add(new StreamEmitter { DefaultEmissionRate = 30 });
outerRingSmoke.Effectors.Add(new StartPositionEffector
{
Distribution = new CircleDistribution { OuterRadius = 5, InnerRadius = 4 }
});
// Smoke in the area inside the ring.
var innerCircleSmoke = CreateSmoke(contentManager);
innerCircleSmoke.Effectors.Add(new StreamEmitter { DefaultEmissionRate = 10 });
innerCircleSmoke.Effectors.Add(new StartPositionEffector
{
Distribution = new CircleDistribution { OuterRadius = 4, InnerRadius = 0 }
});
// Uniform particle parameter that are the same for all child particle systems.
Parameters.AddUniform<float>(ParticleParameterNames.Lifetime).DefaultValue = 5;
_isDepthSortedParameter = Parameters.AddUniform<bool>(ParticleParameterNames.IsDepthSorted);
_isDepthSortedParameter.DefaultValue = true;
Children = new ParticleSystemCollection { outerRingSmoke, innerCircleSmoke };
}
public ModelContainer(Model modelpass, Pose posepass, SkeletonPose skeletonpass)
{
_model = modelpass;
_pose = posepass;
_skeleton = skeletonpass;
var additionalData = (Dictionary<string,object>)_model.Tag;
var animations = (Dictionary<string, SkeletonKeyFrameAnimation>)additionalData["Animations"];
int index = 0;
_animations = new ITimeline[animations.Count];
_animations[0] = new AnimationClip<SkeletonPose>(animations["First"])
{
LoopBehavior = LoopBehavior.Cycle,
Duration = TimeSpan.MaxValue
};
index++;
_animations[1] = new AnimationClip<SkeletonPose>(animations["Second"]);
_animations[2] = new AnimationClip<SkeletonPose>(animations["Second"])
{
LoopBehavior = LoopBehavior.Cycle,
Duration = TimeSpan.MaxValue
};
}
protected HeldPose(IMyoEventGenerator myo, TimeSpan interval, IEnumerable<Pose> targetPoses)
{
Contract.Requires<ArgumentNullException>(myo != null, "myo");
Contract.Requires<ArgumentOutOfRangeException>(interval.TotalMilliseconds >= 0, "interval");
Contract.Requires<ArgumentNullException>(targetPoses != null, "targetPoses");
_targetPoses = new List<Pose>(targetPoses);
if (_targetPoses.Contains(Pose.Unknown))
{
throw new ArgumentException("All target poses must be specified.", "targetPoses");
}
if (_targetPoses.Count < 1)
{
throw new ArgumentException("At least one target pose must be specified.", "targetPoses");
}
_timer = new Timer()
{
AutoReset = true,
Interval = interval.TotalMilliseconds,
};
_timer.Elapsed += Timer_Elapsed;
_lastPose = Pose.Unknown;
_myo = myo;
_myo.PoseChanged += Myo_PoseChanged;
}
public void GetClosestPointCandidates(Vector3F scale, Pose pose, ISpatialPartition<int> otherPartition, Vector3F otherScale, Pose otherPose, Func<int, int, float> callback)
{
if (otherPartition == null)
throw new ArgumentNullException("otherPartition");
if (callback == null)
throw new ArgumentNullException("callback");
// Make sure we are up-to-date.
var otherBasePartition = otherPartition as BasePartition<int>;
if (otherBasePartition != null)
otherBasePartition.UpdateInternal();
else
otherPartition.Update(false);
Update(false);
if (_numberOfItems == 0)
return;
if (otherPartition is ISupportClosestPointQueries<int>)
{
// ----- CompressedAabbTree vs. ISupportClosestPointQueries<int>
GetClosestPointCandidatesImpl(scale, pose, (ISupportClosestPointQueries<int>)otherPartition, otherScale, otherPose, callback);
}
else
{
// ----- CompressedAabbTree vs. *
GetClosestPointCandidatesImpl(otherPartition, callback);
}
}
public DumpContactSetSample(Microsoft.Xna.Framework.Game game)
: base(game)
{
GraphicsScreen.ClearBackground = true;
// Create two collision objects with triangle mesh shapes.
var meshA = new SphereShape(1).GetMesh(0.01f, 4);
var shapeA = new TriangleMeshShape(meshA, true) { Partition = new CompressedAabbTree() };
var poseA = new Pose(new Vector3F(-1, 0, 0), RandomHelper.Random.NextQuaternionF());
var collisionObjectA = new CollisionObject(new GeometricObject(shapeA, poseA));
var meshB = new BoxShape(0.2f, 2, 1f).GetMesh(0.01f, 4);
var shapeB = new TriangleMeshShape(meshB, true) { Partition = new CompressedAabbTree() };
var poseB = new Pose(new Vector3F(0.1f, 0, 0), RandomHelper.Random.NextQuaternionF());
var collisionObjectB = new CollisionObject(new GeometricObject(shapeB, poseB));
// Explicitly create a contact set. (Normally you would get the contact set
// from the collision domain...)
var contactSet = ContactSet.Create(collisionObjectA, collisionObjectB);
// Create a C# sample which visualizes the contact set.
const string Filename = "DumpedContactSet001.cs";
DumpContactSet(contactSet, Filename);
GraphicsScreen.DebugRenderer2D.DrawText(
"Contact set dumped into the file: " + Filename,
new Vector2F(300, 300),
Color.Black);
}
private Droid(Guid id, Pose pose, Guid inventoryID, Logic logic)
: base(id)
{
_pose = pose;
_inventoryID = inventoryID;
_logic = logic;
}
public void Clone()
{
Pose poseA = new Pose(new Vector3F(1, 2, 3));
PointShape pointA = new PointShape(3, 4, 5);
GeometricObject geometryA = new GeometricObject(pointA, poseA);
Pose poseB = new Pose(new Vector3F(1, 2, 3));
PointShape pointB = new PointShape(3, 4, 5);
GeometricObject geometryB = new GeometricObject(pointB, poseB);
MinkowskiSumShape minkowskiSumShape = new MinkowskiSumShape(geometryA, geometryB);
MinkowskiSumShape clone = minkowskiSumShape.Clone() as MinkowskiSumShape;
Assert.IsNotNull(clone);
Assert.IsNotNull(clone.ObjectA);
Assert.IsNotNull(clone.ObjectB);
Assert.AreNotSame(geometryA, clone.ObjectA);
Assert.AreNotSame(geometryB, clone.ObjectB);
Assert.IsTrue(clone.ObjectA is GeometricObject);
Assert.IsTrue(clone.ObjectB is GeometricObject);
Assert.AreEqual(poseA, clone.ObjectA.Pose);
Assert.AreEqual(poseB, clone.ObjectB.Pose);
Assert.IsNotNull(clone.ObjectA.Shape);
Assert.IsNotNull(clone.ObjectB.Shape);
Assert.AreNotSame(pointA, clone.ObjectA.Shape);
Assert.AreNotSame(pointB, clone.ObjectB.Shape);
Assert.IsTrue(clone.ObjectA.Shape is PointShape);
Assert.IsTrue(clone.ObjectB.Shape is PointShape);
Assert.AreEqual(pointA.Position, ((PointShape)clone.ObjectA.Shape).Position);
Assert.AreEqual(pointB.Position, ((PointShape)clone.ObjectB.Shape).Position);
Assert.AreEqual(minkowskiSumShape.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(minkowskiSumShape.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
public Entity ReplacePose(Pose newCurrent) {
var componentPool = GetComponentPool(UnitsComponentIds.Pose);
var component = (PoseComponent)(componentPool.Count > 0 ? componentPool.Pop() : new PoseComponent());
component.Current = newCurrent;
ReplaceComponent(UnitsComponentIds.Pose, component);
return this;
}
/// <summary>
/// Initializes a new instance of the <see cref="PoseEventArgs"/> class.
/// </summary>
/// <param name="myo">The Myo that raised the event. Cannot be <c>null</c>.</param>
/// <param name="timestamp">The timestamp of the event.</param>
/// <param name="pose">The pose that the Myo detected.</param>
/// <exception cref="System.ArgumentNullException">
/// The exception that is thrown when <paramref name="myo"/> is null.
/// </exception>
public PoseEventArgs(IMyo myo, DateTime timestamp, Pose pose)
: base(myo, timestamp)
{
Contract.Requires<ArgumentNullException>(myo != null, "myo");
this.Pose = pose;
}
public void Clone()
{
CompositeShape compositeShape = new CompositeShape();
for (int i = 0; i < 10; i++)
{
Pose pose = new Pose(new Vector3F(i, i, i));
PointShape point = new PointShape(i, i, i);
GeometricObject geometry = new GeometricObject(point, pose);
compositeShape.Children.Add(geometry);
}
CompositeShape clone = compositeShape.Clone() as CompositeShape;
Assert.IsNotNull(clone);
Assert.AreEqual(10, clone.Children.Count);
for (int i = 0; i < 10; i++)
{
Assert.IsNotNull(clone.Children[i]);
Assert.AreNotSame(compositeShape.Children[i], clone.Children[i]);
Assert.IsTrue(clone.Children[i] is GeometricObject);
Assert.AreEqual(compositeShape.Children[i].Pose, clone.Children[i].Pose);
Assert.IsNotNull(clone.Children[i].Shape);
Assert.AreNotSame(compositeShape.Children[i].Shape, clone.Children[i].Shape);
Assert.IsTrue(clone.Children[i].Shape is PointShape);
Assert.AreEqual(((PointShape)compositeShape.Children[i].Shape).Position, ((PointShape)clone.Children[i].Shape).Position);
}
Assert.AreEqual(compositeShape.GetAabb(Pose.Identity).Minimum, clone.GetAabb(Pose.Identity).Minimum);
Assert.AreEqual(compositeShape.GetAabb(Pose.Identity).Maximum, clone.GetAabb(Pose.Identity).Maximum);
}
internal SkinnedModelBoneContent(ushort index, string name, Pose bindPose,
Matrix inverseBindPoseTransform)
{
this.index = index;
this.name = name;
this.bindPose = bindPose;
this.inverseBindPoseTransform = inverseBindPoseTransform;
}
public LinearSequence(AnimationData anim, bool loop)
{
Loop = loop;
this.anim = anim;
Speed = 1.0f;
pose = new Pose(anim.Frames[0]);
length = anim.Frames.Count/anim.Speed;
}
public static Pose Lerp( Pose p1, Pose p2, float t )
{
return new Pose(
Vector3.Lerp( p1.position, p2.position, t ),
Quaternion.Lerp( p1.rotation, p2.rotation, t ),
Vector3.Lerp( p1.scale, p2.scale, t )
);
}
void IConstructable.Construct(IDictionary<string, string> param)
{
var anim = ResourceFactory.LoadAsset<AnimationData>(param["animData"]);
seq = new LinearSequence(anim, true);
var rest = ResourceFactory.LoadAsset<AnimationData>(param["restPose"]).Frames[0];
invRest = new Pose(rest);
pose = new Pose(rest);
invRest.Invert();
}
// Use this for initialization
void Start () {
//the init status of the enmeny
mDir = _CurDir;
mPose = CurPose;
if (Parent != null) {
Parent.Start();
}
}
//change the direction of the character
public void CheckDir(){
if (gameObject == null) {
return;
}
needChangeStatus = false;
if (mDir != _CurDir) {
needChangeStatus = true;
mDir = _CurDir;
}
if (mPose != CurPose) {
needChangeStatus = true;
mPose = CurPose;
}
Quaternion dir = gameObject.transform.localRotation;
float curAngle = dir.eulerAngles.y % 360.0f;
curAngle = curAngle < 0.0f ? curAngle + 360.0f : curAngle;
//rotate the weapon
if (rotateWeapon && CurPose != Pose.Die) {
QuadTextureNgui tex = transform.GetChild(0).GetComponent<QuadTextureNgui>();
if(curAngle >= 0.0f && curAngle <= 151f){
int angle = ((int)(curAngle/10.0f) * 10);
tex.mSpriteName = ""+angle;
tex.mirrorX = false;
tex.InitFace();
} else if (curAngle > 151.0f && curAngle <= 301.0f){
int angle = ((int)((301.0f - curAngle)/10.0f) * 10);
tex.mSpriteName = ""+angle;
tex.mirrorX = true;
tex.InitFace();
} else if(curAngle > 301.0f && curAngle <= 321.0f){
int angle = ((int)((661.0f - curAngle)/10.0f) * 10);
tex.mSpriteName = ""+angle;
tex.mirrorX = true;
tex.InitFace();
}
} else {
//will not be used
if((curAngle >= 0.0f && curAngle < 45.0f) || (curAngle > 315.0f && curAngle <= 360.0f)){
_CurDir = Dir.RightUp;
}
else if(curAngle >= 270.0f && curAngle <= 315.0f){
_CurDir = Dir.LeftUp;
}
else if(curAngle >= 45.0f && curAngle < 90.0f){
_CurDir = Dir.Right;
}
else if(curAngle >= 100.0f && curAngle < 270.0f){
_CurDir = Dir.Left;
}
else if(curAngle >= 127.0f && curAngle < 180.0f){
_CurDir = Dir.LeftDown;
}
else if(curAngle >= 90.0f && curAngle < 127.0f){
_CurDir = Dir.RightDown;
}
}
}
public static Pose random()
{
Pose p = new Pose();
for(int i = 0; i < 20; i++)
{
p.joint_rotations[i] = Random.rotation;
}
return p;
}
public StaticObject(IServiceLocator services, string assetName, Vector3F scale, Pose pose, bool castsShadows, bool addRigidBody)
{
_services = services;
_assetName = assetName;
_scale = scale;
_pose = pose;
_castsShadows = castsShadows;
_addRigidBody = addRigidBody;
}
public void DiscoverSequenceOfPoses()
{
var myo = new Mock<IMyoEventGenerator>(MockBehavior.Strict);
var gestures = new Pose[] { Pose.WaveIn, Pose.WaveOut };
var result = HeldPose.Create(myo.Object, gestures);
Assert.NotNull(result);
}
/// <summary>
/// Applies constant power to both wheels, driving the motor base for a fixed distance, in the current direction
/// </summary>
/// <param name="distance">Distance to travel, in meters</param>
/// <param name="power">Normalized power (torque) value for both wheels</param>
public void DriveDistance(float distance, float power)
{
if (distance < 0)
{
throw new ArgumentOutOfRangeException("distance");
}
_startPoseForDriveDistance = State.Pose;
_distanceToTravel = distance;
SetAxleVelocity(power * _motorTorqueScaling, power * _motorTorqueScaling);
}
// Creates a new rigid body and adds it to the simulation.
private void AddBody(string name, Pose pose, Shape shape, MotionType motionType)
{
var rigidBody = new RigidBody(shape)
{
Name = name,
Pose = pose,
MotionType = motionType,
};
simulation.RigidBodies.Add(rigidBody);
}
///<exclude/>
public Marker()
{
header = new Header();
ns = string.Empty;
pose = new Pose();
scale = new Vector3();
color = new ColorRGBA();
points = new List<Point>();
colors = new List<ColorRGBA>();
text = string.Empty;
mesh_resource = string.Empty;
}
public int Add(Model _model,Pose _pose,SkeletonPose _skel )
{
Map.Add(NumModels, new ICT309Game.Container.ModelContainer(_model, _pose, _skel));
Models.Add(_model);
Poses.Add(_pose);
SkeletonPoses.Add(_skel);
NumModels++;
// Map[0]._model = _model;
// Walk(NumModels-1);
idle(NumModels - 1);
return NumModels - 1;
}
public static Car createCar(Pose pose, Physics ph)
{
// рожаем машину
Car car;
var p = new CarParams();
p.InitDefault();
p.h = 2.5f;
p.h_base = 2f;
car = new Car(p, 0);
car.Create(ph, pose);
return car;
}
public ActionInstance(string filename)
{
poses = new List<Pose>();
// read file
using (StreamReader reader = new StreamReader(filename))
{
// for each 20 lines create pose
string line;
Pose p = new Pose();
int nlines = 0;
int nposes = 0;
Vector3 pos;
while ((line = reader.ReadLine()) != null)
{
string[] vals = line.Split(' ');
pos = new Vector3(float.Parse(vals[0]), float.Parse(vals[1]), float.Parse(vals[2]));
p.joint_locations.Add(pos);
p.data_confidence.Add(float.Parse(vals[3]));
nlines++;
if (nlines % 20 == 0)
{
if (p != null)
{
poses.Add(p);
}
p = new Pose();
nposes++;
}
}
Debug.Log("N poses: " + nposes);
}
// for(int i = 1; i < poses.Count-1; i++)
// {
// Pose p = poses[i];
// for(int c = 0; c < p.data_confidence.Count; c++)
// {
// float cd = p.data_confidence[c];
// p.joint_locations[c] = (Vector3.Slerp(poses[i-1].joint_locations[c], poses[i+1].joint_locations[c], 0.5f));// + cd*p.joint_locations[c];
// }
// }
foreach(Pose p in poses)
{
p.convertLocationToJointRotation();
}
}
public void Create_SinglePoseEnumerable_NewInstance()
{
// Setup
var myo = new Mock<IMyoEventGenerator>(MockBehavior.Strict);
var poses = new Pose[] { Pose.Fist };
// Execute
var result = HeldPose.Create(
myo.Object,
poses);
// Assert
Assert.NotNull(result);
}
private void ReceiveData()
{
client = new UdpClient(port);
while (m_keepRunning)
{
try
{
IPEndPoint anyIP = new IPEndPoint(IPAddress.Any, 0);
byte[] data = client.Receive(ref anyIP);
// Bytes mit der UTF8-Kodierung in das Textformat kodieren.
string text = Encoding.UTF8.GetString(data);
Debug.Log(text);
string[] words = text.Split(' ');
Quaternion q = new Quaternion(float.Parse(words[12]),
float.Parse(words[13]),
float.Parse(words[14]),
float.Parse(words[15]));
Vector3 t = new Vector3(float.Parse(words[18]),
float.Parse(words[19]),
float.Parse(words[20]));
m_newData = new Pose();
UbiMeasurementUtils.coordsysemChange(t, ref m_newData.pos);
UbiMeasurementUtils.coordsysemChange(q, ref m_newData.rot);
}
catch (Exception err)
{
Debug.Log(err.ToString());
}
}
client.Close();
}
internal void Collect(Pose pose)
{
GCHandle gch = GCHandle.Alloc(this);
try
{
// Note: libmyo requires us to stop the hub's thread while collecting
_myo.Hub.StopEventThread();
libmyo.training_collect_data(_handle, (libmyo.PoseType)pose, (libmyo.TrainingCollectStatus)HandleTrainingData, (IntPtr)gch, IntPtr.Zero);
}
finally
{
gch.Free();
_myo.Hub.StartEventThread();
}
}
public void CloneWithPartition()
{
CompositeShape compositeShape = new CompositeShape();
for (int i = 0; i < 10; i++)
{
Pose pose = new Pose(new Vector3F(i, i, i));
PointShape point = new PointShape(i, i, i);
GeometricObject geometry = new GeometricObject(point, pose);
compositeShape.Children.Add(geometry);
}
CloneWithPartition(compositeShape, new AabbTree<int>());
CloneWithPartition(compositeShape, new AdaptiveAabbTree<int>());
CloneWithPartition(compositeShape, new CompressedAabbTree());
CloneWithPartition(compositeShape, new DynamicAabbTree<int>());
CloneWithPartition(compositeShape, new SweepAndPruneSpace<int>());
}
public void Procedure(object ojb)
{
ProcedureBufferToMachine BfToMa = (ProcedureBufferToMachine)ojb;
RobotUnity rb = BfToMa.robot;
// DataBufferToMachine p = BfToMa.points;
TrafficManagementService Traffic = BfToMa.Traffic;
robot.ShowText(" Start -> " + procedureCode);
//GetFrontLineBuffer(false);
// ProRun = false;
rb.mcuCtrl.lampRbOn();
/* frontLinePose = BfToMa.GetFrontLineBuffer();
* if (frontLinePose == null)
* {
* robot.bayId = -1;
* TrafficRountineConstants.ReleaseAll(robot);
* robot.bayId = -1;
* robot.bayIdReg = false;
* robot.orderItem = null;
* robot.SwitchToDetectLine(false);
* if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
* {
* TrafficRountineConstants.RegIntZone_READY.Release(robot);
* robot.robotTag = RobotStatus.IDLE;
* robot.SetSafeYellowcircle(false);
* robot.SetSafeBluecircle(false);
* robot.SetSafeSmallcircle(false);
* robot.TurnOnSupervisorTraffic(false);
* // rb.mcuCtrl.lampRbOff();
* procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY;
* }
* else
* procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_MACHINE;
* ReleaseProcedureHandler(this);
* ProRun = false;
* UpdateInformationInProc(this, ProcessStatus.S);
* order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE;
* order.endTimeProcedure = DateTime.Now;
* order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
* SaveOrderItem(order);
* KillEvent();
* }
* else
* {
* order.frontLinePos = frontLinePose.Position;
* robot.bayId = bayId;
* }*/
while (ProRun)
{
switch (StateBufferToMachine)
{
case BufferToMachine.BUFMAC_IDLE:
//robot.ShowText("BUFMAC_IDLE");
break;
case BufferToMachine.BUFMAC_GET_FRONTLINE:
frontLinePose = BfToMa.GetFrontLineBuffer();
if (frontLinePose == null)
{
if (countFrontLineNull++ < 10)
{
break;
}
countFrontLineNull = 0;
robot.bayId = -1;
TrafficRountineConstants.ReleaseAll(robot);
robot.bayId = -1;
robot.bayIdReg = false;
robot.orderItem = null;
robot.SwitchToDetectLine(false);
robot.ShowText("Error Data Request FrontLine Buffer" + order.dataRequest);
if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position))
{
TrafficRountineConstants.RegIntZone_READY.Release(robot);
robot.robotTag = RobotStatus.IDLE;
robot.SetSafeYellowcircle(false);
robot.SetSafeBluecircle(false);
robot.SetSafeSmallcircle(false);
robot.TurnOnSupervisorTraffic(false);
// rb.mcuCtrl.lampRbOff();
procedureCode = ProcedureCode.PROC_CODE_ROBOT_TO_READY;
}
else
{
procedureCode = ProcedureCode.PROC_CODE_BUFFER_TO_MACHINE;
}
ProRun = false;
robot.ShowText("RELEASED BF---");
UpdateInformationInProc(this, ProcessStatus.S);
order.status = StatusOrderResponseCode.ERROR_GET_FRONTLINE;
order.endTimeProcedure = DateTime.Now;
order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
KillEvent();
ReleaseProcedureHandler(this);
}
else
{
order.frontLinePos = frontLinePose.Position;
robot.bayId = bayId;
StateBufferToMachine = BufferToMachine.BUFMAC_SELECT_BEHAVIOR_ONZONE;
}
//robot.ShowText("BUFMAC_IDLE");
break;
case BufferToMachine.BUFMAC_SELECT_BEHAVIOR_ONZONE:
if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position, TypeZone.OPZS))
{
if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_BACK_FRONTLINE_READY;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = frontLinePose.Position;
//robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : READY");
//robot.ShowText("CHECK - REG");
}
}
else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
{
//robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : VIM");
if (rb.SendPoseStamped(frontLinePose))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_REG;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = frontLinePose.Position;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
//robot.ShowText("CHECK - REG");
}
}
else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
{
Point destPos1 = frontLinePose.Position;
String destName1 = Traffic.DetermineArea(destPos1, TypeZone.MAIN_ZONE);
//robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : OUTER");
if (destName1.Equals("OUTER"))
{
//robot.ShowText("COME FRONTLINE : OUTER");
if (rb.SendPoseStamped(frontLinePose))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos1;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (destName1.Equals("VIM"))
{
//robot.ShowText("COME FRONTLINE : OUTER");
if (rb.SendPoseStamped(frontLinePose))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_REG;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos1;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
}
break;
case BufferToMachine.BUFMAC_ROBOT_GOTO_BACK_FRONTLINE_READY:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
robot.ShowText("Detetect Inside Station Check Ready");
if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT))
{
Stopwatch sw = new Stopwatch();
sw.Start();
do
{
robot.onFlagGoBackReady = true;
if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE || Traffic.RobotIsInArea("READY_FRONTLINE", robot.properties.pose.Position))
{
robot.onFlagGoBackReady = false;
Point destPos2 = frontLinePose.Position;
String destName2 = Traffic.DetermineArea(destPos2, TypeZone.MAIN_ZONE);
if (destName2.Equals("OUTER"))
{
//robot.ShowText("GO FRONTLINE IN OUTER");
if (rb.SendPoseStamped(frontLinePose))
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos2;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (destName2.Equals("VIM"))
{
//robot.ShowText("GO FRONTLINE IN VIM");
if (rb.SendPoseStamped(frontLinePose))
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_FROM_READY;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos2;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
//robot.ShowText("CHECK - REG");
}
}
break;
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
break;
}
if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
break;
}
Thread.Sleep(100);
} while (ProRunStopW);
sw.Stop();
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER_VIM:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
else
{
TrafficRountineConstants.DetectRelease(registryRobotJourney);
}
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY");
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_CHECKIN_BUFFER: // doi robot di den khu vuc checkin cua vung buffer
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY");
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_ZONE_BUFFER_READY: // doi khu vuc buffer san sang de di vao
try
{
if (false == robot.CheckInZoneBehavior(BfToMa.GetAnyPointInBuffer().Position))
{
if (rb.SendPoseStamped(frontLinePose))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_FROM_READY:
TrafficRountineConstants.DetectRelease(registryRobotJourney);
if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
{
break;
}
try
{
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
robot.SwitchToDetectLine(true);
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM_REG:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
else
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM;
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_VIM:
TrafficRountineConstants.DetectRelease(registryRobotJourney);
if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
{
break;
}
try
{
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
robot.SwitchToDetectLine(true);
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER:
// dò và release vùng đăng ky
if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
{
break;
}
TrafficRountineConstants.DetectRelease(registryRobotJourney);
try
{
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
robot.SwitchToDetectLine(true);
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER:
JPallet jPallet_H = BfToMa.GetInfoPallet_H_InBuffer(PistonPalletCtrl.PISTON_PALLET_UP);
jPResult = BfToMa.GetInfoOfPalletBuffer_Compare_W_H(PistonPalletCtrl.PISTON_PALLET_UP, jPallet_H);
String data = JsonConvert.SerializeObject(jPResult.jInfoPallet);
if (rb.SendCmdAreaPallet(data))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER");
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_PICKUP_PALLET_BUFFER:
if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
{
resCmd = ResponseCommand.RESPONSE_NONE;
BfToMa.UpdatePalletState(PalletStatus.F, jPResult.palletId, order.planId);
//Reset trạng thai W cho pallet_H nếu truoc đó có cây W
if (jPResult.jInfoPallet.row < order.palletRow)
{
BfToMa.UpdatePalletState(PalletStatus.W, order.palletId_H, order.planId);
}
onUpdatedPalletState = true;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER: // đợi
try
{
if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
{
robot.bayId = -1;
robot.bayIdReg = false;
robot.ReleaseWorkingZone();
robot.SwitchToDetectLine(false);
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_SELECT_ZONE;
// cập nhật lại điểm xuất phát
registryRobotJourney.startPoint = robot.properties.pose.Position;
//robot.ShowText("BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_SELECT_ZONE:
String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE);
Pose destPos = BfToMa.GetFrontLineMachine();
String destName = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE);
if (startNamePoint.Equals("VIM"))
{
if (rb.SendPoseStamped(destPos))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM_REG;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos.Position;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (startNamePoint.Equals("OUTER"))
{
if (destName.Equals("OUTER"))
{
if (rb.SendPoseStamped(destPos))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos.Position;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (destName.Equals("VIM"))
{
if (rb.SendPoseStamped(destPos))
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM_REG;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos.Position;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
}
break;
case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM_REG:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
else
{
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM;
}
break;
case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET_IN_VIM:
try
{
TrafficRountineConstants.DetectRelease(registryRobotJourney);
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
robot.SwitchToDetectLine(true);
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET;
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_GOTO_FRONTLINE_DROPDOWN_PALLET:
try
{
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
robot.SwitchToDetectLine(true);
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET;
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_SEND_CMD_DROPDOWN_PALLET:
if (rb.SendCmdAreaPallet(BfToMa.GetInfoOfPalletMachine(PistonPalletCtrl.PISTON_PALLET_DOWN)))
{
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET");
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_DROPDOWN_PALLET:
if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE;
//robot.ShowText("BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_WAITTING_GOTO_FRONTLINE:
if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
{
robot.SwitchToDetectLine(false);
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_RELEASED;
//robot.ShowText("BUFMAC_ROBOT_RELEASED");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToMachine.BUFMAC_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới
TrafficRountineConstants.ReleaseAll(robot);
robot.bayId = -1;
robot.bayIdReg = false;
robot.orderItem = null;
robot.SwitchToDetectLine(false);
// Release WorkinZone Robot
// robot.robotTag = RobotStatus.IDLE;
rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_MACHINE;
// if (errorCode == ErrorCode.RUN_OK) {
// } else {
// ErrorProcedureHandler (this);
// }
ProRun = false;
//robot.ShowText("RELEASED");
UpdateInformationInProc(this, ProcessStatus.S);
order.status = StatusOrderResponseCode.FINISHED;
order.endTimeProcedure = DateTime.Now;
order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
KillEvent();
ReleaseProcedureHandler(this);
break;
case BufferToMachine.BUFMAC_ROBOT_DESTROY:
order.endTimeProcedure = DateTime.Now;
order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
robot.SwitchToDetectLine(false);
robot.ReleaseWorkingZone();
// StateBufferToMachine = BufferToMachine.BUFMAC_ROBOT_RELEASED;
//robot.prioritLevel.OnAuthorizedPriorityProcedure = false;
ProRun = false;
UpdateInformationInProc(this, ProcessStatus.F);
order.status = StatusOrderResponseCode.ROBOT_ERROR;
//reset status pallet Faile H->Ws
selectHandleError = SelectHandleError.CASE_ERROR_EXIT;
procedureStatus = ProcedureStatus.PROC_KILLED;
//FreeHoldBuffer(order.palletId_H);
KillEvent();
//this.robot.DestroyRegistrySolvedForm();
break;
default:
break;
}
// robot.ShowText("-> " + procedureCode);
Thread.Sleep(700);
}
StateBufferToMachine = BufferToMachine.BUFMAC_IDLE;
}
/// <summary>
/// Concatenate two transform poses.
/// </summary>
/// <param name="lhs">The pose to prepend.</param>
/// <param name="rhs">The pose to append.</param>
/// <returns>The concatenated pose.</returns>
private static Pose Multiply(Pose lhs, Pose rhs)
{
return(new Pose(lhs.position + lhs.rotation * rhs.position, lhs.rotation * rhs.rotation));
}
public MotionFrameInfo(TrajectoryInfo trajectoryInfo, Pose pose, int tag)
{
this.trajectoryInfo = trajectoryInfo;
this.pose = pose;
this.tag = tag;
}
void Placing()
{
if (!placed) //if it is the first phase
{
if (cooldown <= 0)
{
foreach (Touch touch in Input.touches)
{
if (touch.phase == TouchPhase.Began || touch.phase == TouchPhase.Moved || touch.phase == TouchPhase.Ended)
{
if (raycastManager.Raycast(touch.position, hits, UnityEngine.XR.ARSubsystems.TrackableType.PlaneWithinBounds))
{
if (marker.activeSelf == false)
{
marker.SetActive(true);
}
//Pose p = new Pose(hits[0].pose.position + (Vector3.up * height / 100), hits[0].pose.rotation); //move marker to new hit
marker.transform.position = hits[0].pose.position + (Vector3.up * height / 100);
marker.transform.rotation = hits[0].pose.rotation;
marker.transform.localScale = new Vector3(scale, scale, scale) * defaultScale;
if (touch.phase == TouchPhase.Ended) //if the lifting of the touch is on a plane go to the rotation screen
{
plane = planeManager.GetPlane(hits[0].trackableId);
startOriginPose = new Pose(hits[0].pose.position, hits[0].pose.rotation);
confirmUI.SetActive(true);
placed = true;
adjust = true;
foreach (ARPlane pl in planeManager.trackables)
{
if (pl != plane)
{
pl.gameObject.SetActive(false);
}
}
planeManager.detectionMode = UnityEngine.XR.ARSubsystems.PlaneDetectionMode.None;
PopupManager.popupManager.DisplayPopup(PopupManager.FOUND, false);
mainPlane = plane.gameObject;
//mainPlane.GetComponent<MeshRenderer>().enabled = false;
//mainPlane.GetComponent<LineRenderer>().enabled = false;
}
}
else
{
marker.SetActive(false);
}
}
}
}
else
{
cooldown -= Time.deltaTime;
}
}
else if (adjust) //if it is the second phase
{
CheckMarker();
marker.transform.position = startOriginPose.position + (Vector3.up * height / 100);
marker.transform.localEulerAngles = startOriginPose.rotation.eulerAngles + new Vector3(0, rotation, 0);
marker.transform.localScale = new Vector3(scale, scale, scale) * defaultScale;
//Debug.Log((Vector3.up * height / 100));
if (done)
{
Destroy(marker.gameObject);
startOriginPose.position += (Vector3.up * height / 100);
anchor = refManager.AttachReferencePoint(plane, startOriginPose).gameObject;
print("Anchor placed");
done = adjust = false;
confirmUI.SetActive(false);
if (MySceneManager.CurrentScene == MySceneManager.CHOOSINGSTART)
{
MySceneManager.sceneManager.ChangeScene(MySceneManager.MAPSELECTOR);
}
else if (MySceneManager.CurrentScene == MySceneManager.CHOOSINGPOS)
{
MySceneManager.sceneManager.ChangeScene(MySceneManager.MAIN);
}
}
}
}
public PoseEventArgs(Myo myo, DateTime timestamp, Pose pose)
: base(myo, timestamp)
{
this.Pose = pose;
}
/// <summary>
/// Takes a correctly formatted array and returns a pose from that array.
/// </summary>
/// <param name="poseArray">Array of floats that encodes a pose.</param>
/// <param name="pose">Pose encoded in the float array.</param>
public static void ArrayToPose(float[] poseArray, ref Pose pose)
{
pose.position = new Vector3(poseArray[0], poseArray[1], poseArray[2]);
pose.rotation = new Quaternion(poseArray[3], poseArray[4], poseArray[5], poseArray[6]);
}
/// <summary>
/// Calls body.MovePosition() and body.MoveRotation() using the argument Pose.
/// </summary
public static void MovePose(this Rigidbody body, Pose pose)
{
body.MovePosition(pose.position);
body.MoveRotation(pose.rotation);
}
/// <summary>
/// Attempts to create a new anchor "attached" to the trackable with id <paramref name="trackableToAffix"/>.
/// The behavior of the anchor depends on the type of trackable to which this anchor is attached.
/// </summary>
/// <param name="trackableToAffix">The id of the trackable to which to attach.</param>
/// <param name="pose">The pose, in session space, of the anchor to create.</param>
/// <param name="anchor">The new anchor. Only valid if this method returns <c>true</c>.</param>
/// <returns><c>true</c> if the new anchor was added, otherwise <c>false</c>.</returns>
public bool TryAttachAnchor(TrackableId trackableToAffix, Pose pose, out XRAnchor anchor)
{
return(provider.TryAttachAnchor(trackableToAffix, pose, out anchor));
}
/// <summary>
/// Attempts to create a new anchor with the provide <paramref name="pose"/>.
/// </summary>
/// <param name="pose">The pose, in session space, of the new anchor.</param>
/// <param name="anchor">The new anchor. Only valid if this method returns <c>true</c>.</param>
/// <returns><c>true</c> if the new anchor was added, otherwise <c>false</c>.</returns>
public bool TryAddAnchor(Pose pose, out XRAnchor anchor)
{
return(provider.TryAddAnchor(pose, out anchor));
}
void ReadPose()
{
if (PosFile[UnitId] != null)
{
Pose current = null;
PosAction curAct = null;
AttackDes att = null;
DragDes dra = null;
NextPose nex = null;
int left = 0;
int leftAct = 0;
int leftAtt = 0;
int leftDra = 0;
int leftNex = 0;
string text = System.Text.Encoding.ASCII.GetString(PosFile[UnitId].bytes);
string[] pos = text.Split(new char[] { '\r', '\n' }, System.StringSplitOptions.RemoveEmptyEntries);
for (int i = 0; i < pos.Length; i++)
{
if (current != null && current.Idx == 573)
{
//Debug.Log("get");
}
string line = pos[i];
string[] lineObject = line.Split(new char[] { ' ' }, System.StringSplitOptions.RemoveEmptyEntries);
if (lineObject.Length == 0)
{
//Debug.Log("line i:" + i);
//空行跳过
continue;
}
else if (lineObject[0].StartsWith("#"))
{
continue;
}
else
if (lineObject[0] == "Pose" && left == 0 && leftAct == 0)
{
Pose insert = new Pose();
ActionList[UnitId].Add(insert);
int idx = int.Parse(lineObject[1]);
insert.Idx = idx;
current = insert;
}
else if (lineObject[0] == "{")
{
if (nex != null)
{
leftNex++;
}
else
if (dra != null)
{
leftDra++;
}
else
if (att != null)
{
leftAtt++;
}
else
if (curAct != null)
{
leftAct++;
}
else
{
left++;
}
}
else if (lineObject[0] == "}")
{
if (nex != null)
{
leftNex--;
if (leftNex == 0)
{
nex = null;
}
}
else
if (dra != null)
{
leftDra--;
if (leftDra == 0)
{
dra = null;
}
}
else
if (att != null)
{
leftAtt--;
if (leftAtt == 0)
{
att = null;
}
}
else
if (curAct != null)
{
leftAct--;
if (leftAct == 0)
{
curAct = null;
}
}
else
{
left--;
if (left == 0)
{
current = null;
}
}
}
else if (lineObject[0] == "link" || lineObject[0] == "Link" || lineObject[0] == "Link\t" || lineObject[0] == "link\t")
{
current.Link = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "source" || lineObject[0] == "Source")
{
current.SourceIdx = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "Start" || lineObject[0] == "start")
{
if (nex != null)
{
nex.Start = int.Parse(lineObject[1]);
}
else
if (dra != null)
{
dra.Start = int.Parse(lineObject[1]);
}
else
if (att != null)
{
att.Start = int.Parse(lineObject[1]);
}
else
if (curAct != null)
{
curAct.Start = int.Parse(lineObject[1]);
}
else
{
current.Start = int.Parse(lineObject[1]);
}
}
else if (lineObject[0] == "End" || lineObject[0] == "end")
{
if (nex != null)
{
nex.End = int.Parse(lineObject[1]);
}
else
if (dra != null)
{
dra.End = int.Parse(lineObject[1]);
}
else
if (att != null)
{
att.End = int.Parse(lineObject[1]);
}
else
if (curAct != null)
{
curAct.End = int.Parse(lineObject[1]);
}
else
{
current.End = int.Parse(lineObject[1]);
}
}
else if (lineObject[0] == "Speed" || lineObject[0] == "speed")
{
if (curAct != null)
{
curAct.Speed = float.Parse(lineObject[1]);
}
}
else if (lineObject[0] == "LoopStart")
{
current.LoopStart = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "LoopEnd")
{
current.LoopEnd = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "EffectType")
{
current.EffectType = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "EffectID")
{
current.EffectID = lineObject[1];
}
else if (lineObject[0] == "Blend")
{
PosAction act = new PosAction();
act.Type = "Blend";
current.ActionList.Add(act);
curAct = act;
}
else if (lineObject[0] == "Action")
{
PosAction act = new PosAction();
act.Type = "Action";
current.ActionList.Add(act);
curAct = act;
}
else if (lineObject[0] == "Attack")
{
att = new AttackDes();
att.PoseIdx = current.Idx;
current.Attack.Add(att);
}
else if (lineObject[0] == "bone")
{
//重新分割，=号分割，右边的,号分割
lineObject = line.Split(new char[] { '=' }, System.StringSplitOptions.RemoveEmptyEntries);
string bones = lineObject[1];
while (bones.EndsWith(","))
{
i++;
lineObject = new string[1];
lineObject[0] = pos[i];
bones += lineObject[0];
}
//bones = bones.Replace(' ', '_');
string[] bonesstr = bones.Split(new char[] { ',' });
for (int j = 0; j < bonesstr.Length; j++)
{
string b = bonesstr[j].TrimStart(new char[] { ' ', '\"' });
b = b.TrimEnd(new char[] { '\"', ' ' });
b = b.Replace(' ', '_');
att.bones.Add(b);
}
}
else if (lineObject[0] == "AttackType")
{
att._AttackType = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "CheckFriend")
{
att.CheckFriend = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "DefenseValue")
{
att.DefenseValue = float.Parse(lineObject[1]);
}
else if (lineObject[0] == "DefenseMove")
{
att.DefenseMove = float.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetValue")
{
att.TargetValue = float.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetMove")
{
att.TargetMove = float.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetPose")
{
att.TargetPose = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetPoseFront")
{
att.TargetPoseFront = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetPoseBack")
{
att.TargetPoseBack = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetPoseLeft")
{
att.TargetPoseLeft = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "TargetPoseRight")
{
att.TargetPoseRight = int.Parse(lineObject[1]);
}
else if (lineObject[0] == "Drag")
{
dra = new DragDes();
current.Drag = dra;
}
else if (lineObject[0] == "Time")
{
if (nex != null)
{
nex.Time = float.Parse(lineObject[1]);
}
else
{
dra.Time = float.Parse(lineObject[1]);
}
}
else if (lineObject[0] == "Color")
{
string[] rgb = lineObject[1].Split(new char[] { ',' }, System.StringSplitOptions.RemoveEmptyEntries);
dra.Color.x = int.Parse(rgb[0]);
dra.Color.y = int.Parse(rgb[1]);
dra.Color.z = int.Parse(rgb[2]);
}
else if (lineObject[0] == "NextPose")
{
current.Next = new NextPose();
nex = current.Next;
}
else if (lineObject[0] == "{}")
{
current = null;
continue;
}
else
{
Debug.Log("line :" + i + " can t understand：" + pos[i]);
break;
}
}
}
}
//动作在地面还是空中
//动作是移动 防守 还是攻击 受伤 待机
public void ChangeAction(int idx = CommonAction.Idle)
{
if (ActionList[UnitId].Count > idx && load != null)
{
CanRotate = CanMove = CanJump = CanDefence = CanChangeWeapon = false;
if (idx == CommonAction.Defence)
{
//switch ((EquipWeaponType)_Self.GetCurEquipType())
//{
// case EquipWeaponType.Knife: idx = Data.CommonActionI.KnifeDefence;break;
// case EquipWeaponType.Sword: idx = Data.CommonActionI.SwordDefence; break;
// case EquipWeaponType.Blade: idx = Data.CommonActionI.BladeDefence; break;
// case EquipWeaponType.Lance: idx = Data.CommonActionI.LanceDefence; break;
// case EquipWeaponType.Brahchthrust: idx = Data.CommonActionI.BrahchthrustDefence;break;
// case EquipWeaponType.Dart: idx = Data.CommonActionI.DartDefence;break;
// case EquipWeaponType.Gloves: return;//没找到
// case EquipWeaponType.Guillotines: idx = Data.CommonActionI.GuillotinesDefence;break;
// case EquipWeaponType.Hammer: idx = Data.CommonActionI.HammerDefence;break;
// case EquipWeaponType.NinjaSword:return;
// case EquipWeaponType.HeavenLance:return;//乾坤3个都没找到防御姿势
// case EquipWeaponType.Gun: return;//火枪是没有防御动作的
//}
}
else
if (idx == CommonAction.Jump)
{
CanChangeWeapon = true; //空中可换武器
CanMove = true; //跳跃后，可以移动
}
//else if (idx == Data.CommonActionI.ShortJump)
//{
// CanChangeWeapon = false;//空中不可换武器
// CanMove = true;
//}
else if (idx == CommonAction.WalkBackward)
{
CanMove = true;
}
else if (idx == CommonAction.WalkLeft)
{
CanMove = true;
}
else if (idx == CommonAction.WalkRight)
{
CanMove = true;
}
else if (idx == CommonAction.Run)
{
CanMove = true;
CanJump = true;
}
else if (idx == CommonAction.Idle)
{
CanRotate = CanMove = CanJump = CanDefence = CanChangeWeapon = true;
}
load.SetPosData(ActionList[UnitId][idx]);
mActiveAction = ActionList[UnitId][idx];
if (idx != 0)
{
CanDefence = false;
}
else
{
CanDefence = true;
}
}
}
internal static extern void Internal_GetCurrentPose(IntPtr obj, ref Pose pose);
private static extern void SetPoseForSubmit(int layerId, Pose pose);
private bool ArePosesEqual(Pose a, Pose b)
{
return(AreVector3sEqual(a.position, b.position) &&
AreQuaternionsEqual(a.rotation, b.rotation));
}
/// <summary>
/// Co-routine to lerp the controller to standing size
/// </summary>
private IEnumerator ControllerToStanding()
{
// Get the target height from the heightStand trigger
float fFinishHeight = m_HeightStand.GetComponent <CapsuleCollider>().height;
// Wait untill clear to stand
bool bCantStand = true;
while (bCantStand)
{
// If grounded, check the heightStand trigger
if (m_eJump == Jump.Grounded)
{
bCantStand = m_HeightStand.Colliding;
if (m_HeightStand.OnlyDynamicCollisions)
{
m_HeightStand.ForceOutRigidbodies();
}
}
// Not grounded, then there is space
else
{
bCantStand = false;
}
// Keep waiting.
yield return(new WaitForEndOfFrame());
}
// Store the previous pose
m_ePreviousPose = m_ePose;
// Set the current pose
m_ePose = Pose.Stand;
// Initialise timer
m_fPoseTimer = 0.0f;
// Set the lerping flag
m_bPoseLerping = true;
// Current height is stored as the start height as to lerp from any beginning
float fStartHeight = m_CharacterController.height;
// Continue to lerp towards target height while unobstructed
while (m_fPoseTimer < m_fPoseTime)
{
// If new collisions detected, wait.
if (m_HeightStand.Colliding)
{
yield return(new WaitForEndOfFrame());
if (m_HeightStand.OnlyDynamicCollisions)
{
m_HeightStand.ForceOutRigidbodies();
}
}
else
{
// ... else Lerp
m_fPoseTimer += Time.deltaTime;
m_CharacterController.height = Mathf.Lerp(fStartHeight, fFinishHeight, m_fPoseTimer / m_fPoseTime);
m_CharacterController.center = Vector3.up * m_CharacterController.height * 0.5f;
m_POVCamera.transform.parent.localPosition = Vector3.up * (1 - m_fEyeDepthRatio) * m_CharacterController.height;
yield return(new WaitForEndOfFrame());
}
}
// Once finished, snap to height, centre, and camera, to remove any inaccuracy
m_CharacterController.height = fFinishHeight;
m_CharacterController.center = Vector3.up * m_CharacterController.height * 0.5f;
m_POVCamera.transform.parent.localPosition = Vector3.up * (1 - m_fEyeDepthRatio) * m_CharacterController.height;
m_bPoseLerping = false;
}
/// <summary>
/// Should create a new anchor with the provided <paramref name="pose"/>.
/// </summary>
/// <param name="pose">The pose, in session space, of the new anchor.</param>
/// <param name="anchor">The new anchor. Must be valid only if this method returns <c>true</c>.</param>
/// <returns>Should return <c>true</c> if the new anchor was added, otherwise <c>false</c>.</returns>
public virtual bool TryAddAnchor(Pose pose, out XRAnchor anchor)
{
anchor = default(XRAnchor);
return(false);
}
void Update()
{
if (placementTimer >= generateAfterSeconds)
{
placementTimer = 0;
GroupBalls();
}
else
{
placementTimer += Time.deltaTime * 1.0f;
}
if (Input.touchCount == 0)
{
return;
}
Touch touch = Input.GetTouch(0);
if (UnityEngine.EventSystems.EventSystem.current.IsPointerOverGameObject(touch.fingerId))
{
return;
}
if (touch.phase != TouchPhase.Began)
{
return;
}
Ray ray = arCamera.ScreenPointToRay(touch.position);
RaycastHit hitObject;
if (Physics.Raycast(ray, out hitObject))
{
debugLog.gameObject.SetActive(true);
removeBallButton.gameObject.SetActive(true);
ChangeColor(Color.gray);
GroupBalls();
GameObject obj = hitObject.transform.gameObject;
selectedBall = obj;
ChangeColor(Color.red);
}
else
{
if (selectedBall != null)
{
ChangeColor(Color.gray);
GroupBalls();
selectedBall = null;
removeBallButton.gameObject.SetActive(false);
return;
}
if (arRaycastManager.Raycast(touch.position, hits, TrackableType.FeaturePoint))
{
Pose hitPose = hits[0].pose;
ARReferencePoint referencePoint = arReferencePointManager.AddReferencePoint(hitPose);
if (referencePoint == null)
{
debugLog.gameObject.SetActive(true);
string errorEntry = "There was an error creating a reference point\n";
Debug.Log(errorEntry);
debugLog.text += errorEntry;
}
else
{
referencePoints.Add(referencePoint);
referencePointCount.text = $"Reference Point Count: {referencePoints.Count}";
}
}
}
}
public Car(Pose pose) => (Pose, NextTurn) = (pose, Direction.Left);
private void UpdateTransformFromPose(Transform eyeTransform, Pose eyePose)
{
eyeTransform.localPosition = eyePose.position;
eyeTransform.localRotation = eyePose.rotation;
}
public void Procedure(object ojb)
{
ProcedureBufferToGate BuffToGate = (ProcedureBufferToGate)ojb;
RobotUnity rb = BuffToGate.robot;
// DataBufferToGate p = BuffToGate.points;
doorServiceCtrl = new DoorServiceCtrl();
doorServiceCtrl = getDoorService();
ds = doorServiceCtrl.doorService;
ds.setRb(rb);
TrafficManagementService Traffic = BuffToGate.Traffic;
rb.mcuCtrl.lampRbOn();
robot.ShowText(" Start -> " + procedureCode);
while (ProRun)
{
switch (StateBufferToGate)
{
case BufferToGate.BUFGATE_IDLE:
break;
case BufferToGate.BUFGATE_SELECT_BEHAVIOR_ONZONE:
if (Traffic.RobotIsInArea("READY", robot.properties.pose.Position, TypeZone.OPZS))
{
if (rb.PreProcedureAs == ProcedureControlAssign.PRO_READY)
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_GOTO_BACK_FRONTLINE_READY;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = BuffToGate.GetFrontLineBuffer().Position;
////robot.ShowText("BUFMAC_SELECT_BEHAVIOR_ONZONE : READY");
////robot.ShowText("CHECK - REG");
}
}
else if (Traffic.RobotIsInArea("VIM", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
{
if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = BuffToGate.GetFrontLineBuffer().Position;
////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (Traffic.RobotIsInArea("OUTER", robot.properties.pose.Position, TypeZone.MAIN_ZONE))
{
Point destPos1 = BuffToGate.GetFrontLineBuffer().Position;
String destName1 = Traffic.DetermineArea(destPos1, TypeZone.MAIN_ZONE);
if (destName1.Equals("OUTER"))
{
////robot.ShowText("GO FRONTLINE IN OUTER");
if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos1;
////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (destName1.Equals("VIM"))
{
////robot.ShowText("GO FRONTLINE IN VIM");
if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_REG;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos1;
////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
////robot.ShowText("CHECK - REG");
}
}
}
break;
case BufferToGate.BUFGATE_ROBOT_GOTO_BACK_FRONTLINE_READY:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
////robot.ShowText("START :GOTO_BACK_FRONTLINE_READY");
if (rb.SendCmdPosPallet(RequestCommandPosPallet.REQUEST_GOBACK_FRONTLINE_TURN_RIGHT))
{
Stopwatch sw = new Stopwatch();
sw.Start();
do
{
robot.onFlagGoBackReady = true;
if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
{
robot.onFlagGoBackReady = false;
Pose destPos2 = BuffToGate.GetFrontLineBuffer();
String destName2 = Traffic.DetermineArea(destPos2.Position, TypeZone.MAIN_ZONE);
if (destName2.Equals("OUTER"))
{
////robot.ShowText("GO FRONTLINE IN OUTER");
if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos2.Position;
////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
else if (destName2.Equals("VIM"))
{
////robot.ShowText("GO FRONTLINE IN VIM");
if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_READY;
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position, TypeZone.OPZS);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = destPos2.Position;
////robot.ShowText("BUFMAC_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
////robot.ShowText("CHECK - REG");
}
}
break;
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
break;
}
if (sw.ElapsedMilliseconds > TIME_OUT_WAIT_GOTO_FRONTLINE)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
break;
}
Thread.Sleep(100);
} while (ProRunStopW);
sw.Stop();
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_ZONE_BUFFER_READY: // doi khu vuc buffer san sang de di vao
try
{
if (false == robot.CheckInZoneBehavior(BuffToGate.GetFrontLineBuffer().Position))
{
if (rb.SendPoseStamped(BuffToGate.GetFrontLineBuffer()))
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER");
}
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_READY:
TrafficRountineConstants.DetectRelease(registryRobotJourney);
try
{
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM_REG:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
else
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM;
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER_FROM_VIM:
TrafficRountineConstants.DetectRelease(registryRobotJourney);
if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
{
break;
}
try
{
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_CAME_FRONTLINE_BUFFER:
try
{
if (TrafficCheckInBuffer(goalFrontLinePos, bayId))
{
break;
}
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER;
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
}
catch (System.Exception)
{
errorCode = ErrorCode.CAN_NOT_GET_DATA;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_SEND_CMD_PICKUP_PALLET_BUFFER:
String palletInfo = JsonConvert.SerializeObject(BuffToGate.GetInfoOfPalletBuffer(PistonPalletCtrl.PISTON_PALLET_UP));
if (rb.SendCmdAreaPallet(palletInfo))
{
// rb.SendCmdLineDetectionCtrl(RequestCommandLineDetect.REQUEST_LINEDETECT_PALLETUP);
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_PICKUP_PALLET_BUFFER;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_PICKUP_PALLET_BUFFER");
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_PICKUP_PALLET_BUFFER:
if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETUP)
{
resCmd = ResponseCommand.RESPONSE_NONE;
BuffToGate.UpdatePalletState(PalletStatus.F, order.palletId_H, order.planId);
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_BUFFER: // đợi
if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
{
robot.bayId = -1;
robot.bayIdReg = false;
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = false;
StateBufferToGate = BufferToGate.BUFGATE_SELECT_BEHAVIOR_ONZONE_TO_GATE;
// cập nhật lại điểm xuất phát
registryRobotJourney.startPoint = robot.properties.pose.Position;
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_SELECT_BEHAVIOR_ONZONE_TO_GATE:
String startNamePoint = Traffic.DetermineArea(registryRobotJourney.startPoint, TypeZone.MAIN_ZONE);
Pose destPos = doorServiceCtrl.PointFrontLine;
String destName = Traffic.DetermineArea(destPos.Position, TypeZone.MAIN_ZONE);
// đi tới đầu line cổng theo tọa độ chỉ định. gate 1 , 2, 3
if (rb.SendPoseStamped(doorServiceCtrl.PointFrontLine))
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG;
// Cap Nhat Thong Tin CHuyen Di
registryRobotJourney.startPlaceName = Traffic.DetermineArea(robot.properties.pose.Position);
registryRobotJourney.startPoint = robot.properties.pose.Position;
registryRobotJourney.endPoint = doorServiceCtrl.PointFrontLine.Position;
////robot.ShowText("FORBUF_ROBOT_WAITTING_GOTO_GATE");
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_REG:
if (TrafficRountineConstants.DetetectInsideStationCheck(registryRobotJourney))
{
break;
}
else
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM;
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM:
TrafficRountineConstants.DetectRelease(registryRobotJourney);
if (Traffic.RobotIsInArea("C5", rb.properties.pose.Position))
{
ds.setDoorBusy(true);
ds.openDoor(DoorService.DoorType.DOOR_BACK);
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR;
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE_FROM_VIM_OPEN_DOOR:
TrafficRountineConstants.DetectRelease(registryRobotJourney);
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_CAME_GATE_POSITION;
////robot.ShowText("BUFGATE_ROBOT_CAME_GATE_POSITION");
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_GOTO_GATE:
if (resCmd == ResponseCommand.RESPONSE_LASER_CAME_POINT)
{
resCmd = ResponseCommand.RESPONSE_NONE;
//rb.prioritLevel.OnAuthorizedPriorityProcedure = true;
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_CAME_GATE_POSITION;
////robot.ShowText("BUFGATE_ROBOT_CAME_GATE_POSITION");
}
break;
case BufferToGate.BUFGATE_ROBOT_CAME_GATE_POSITION: // da den khu vuc cong , gui yeu cau mo cong.
// ds.setDoorBusy(true);
// ds.openDoor(DoorService.DoorType.DOOR_BACK);
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_OPEN_DOOR;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_OPEN_DOOR");
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_OPEN_DOOR: //doi mo cong
RetState ret = ds.checkOpen(DoorService.DoorType.DOOR_BACK);
if (ret == RetState.DOOR_CTRL_SUCCESS)
{
if (rb.SendCmdAreaPallet(doorServiceCtrl.infoPallet))
{
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER");
}
}
else if (ret == RetState.DOOR_CTRL_ERROR)
{
robot.ShowText("BUFGATE_ROBOT_WAITTING_OPEN_DOOR_ERROR__(-_-)");
Thread.Sleep(1000);
ds.setDoorBusy(true);
ds.openDoor(DoorService.DoorType.DOOR_BACK);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_DROPDOWN_PALLET_BUFFER: // doi robot gap hang
if (resCmd == ResponseCommand.RESPONSE_LINEDETECT_PALLETDOWN)
{
resCmd = ResponseCommand.RESPONSE_NONE;
// BuffToGate.UpdatePalletState(PalletStatus.W);
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_GOBACK_FRONTLINE_GATE:
if (resCmd == ResponseCommand.RESPONSE_FINISH_GOBACK_FRONTLINE)
{
resCmd = ResponseCommand.RESPONSE_NONE;
ds.closeDoor(DoorService.DoorType.DOOR_BACK);
ds.setDoorBusy(false);
ds.LampSetStateOn(DoorType.DOOR_BACK);
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_WAITTING_CLOSE_GATE;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_CLOSE_GATE");
}
else if (resCmd == ResponseCommand.RESPONSE_ERROR)
{
errorCode = ErrorCode.DETECT_LINE_ERROR;
CheckUserHandleError(this);
}
break;
case BufferToGate.BUFGATE_ROBOT_WAITTING_CLOSE_GATE: // doi dong cong.
StateBufferToGate = BufferToGate.BUFGATE_ROBOT_RELEASED;
////robot.ShowText("BUFGATE_ROBOT_WAITTING_CLOSE_GATE");
break;
case BufferToGate.BUFGATE_ROBOT_RELEASED: // trả robot về robotmanagement để nhận quy trình mới
ds.removeListCtrlDoorBack();
robot.bayId = -1;
robot.bayIdReg = false;
TrafficRountineConstants.ReleaseAll(robot);
robot.robotTag = RobotStatus.IDLE;
rb.PreProcedureAs = ProcedureControlAssign.PRO_BUFFER_TO_GATE;
ReleaseProcedureHandler(this);
ProRun = false;
UpdateInformationInProc(this, ProcessStatus.S);
order.endTimeProcedure = DateTime.Now;
order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
KillEvent();
break;
case BufferToGate.BUFGATE_ROBOT_DESTROY:
ds.removeListCtrlDoorBack();
ProRunStopW = false;
ProRunStopW = false;
robot.robotTag = RobotStatus.IDLE;
ProRun = false;
order.endTimeProcedure = DateTime.Now;
order.totalTimeProcedure = order.endTimeProcedure.Subtract(order.startTimeProcedure).TotalMinutes;
TrafficRountineConstants.ReleaseAll(robot);
break;
default:
break;
}
//robot.ShowText("-> " + procedureCode);
Thread.Sleep(500);
}
StateBufferToGate = BufferToGate.BUFGATE_IDLE;
}
/// <summary>
/// Sets body.position and body.rotation using the argument Pose.
/// </summary
public static void SetPose(this Rigidbody body, Pose pose)
{
body.position = pose.position;
body.rotation = pose.rotation;
}
private void InvokeHoldCompleted(InteractionSource source, InteractionSourcePose sourcePose, Pose headPose)
{
var holdCompletedEvent = HoldCompletedEvent;
if (holdCompletedEvent != null)
{
holdCompletedEvent(source.m_SourceKind, new Ray(headPose.position, headPose.rotation * Vector3.forward));
}
var holdCompleted = HoldCompleted;
if (holdCompleted != null)
{
HoldCompletedEventArgs eventArgs;
eventArgs.m_Source = source;
eventArgs.m_SourcePose = sourcePose;
eventArgs.m_HeadPose = headPose;
holdCompleted(eventArgs);
}
}
internal void SetGlobalPose(Pose pose)
{
_dummyTransform.SetGlobalPose(pose);
}
/// <summary>
/// Update is called by the update proxy.
/// </summary>
private void Update()
{
ErrorStatus = "";
if (hasPendingLoadTask)
{
ErrorStatus = "pending background load task";
return;
}
// AnchorManager.Update takes care of creating anchors&edges and feeding the up-to-date state
// into the FrozenWorld engine
bool hasSpongyAnchors = AnchorManager.Update();
if (!hasSpongyAnchors)
{
// IFragmentManager.Pause() will set all fragments to disconnected.
ErrorStatus = AnchorManager.ErrorStatus;
FragmentManager.Pause();
return;
}
try
{
DiagnosticRecordings.Update();
}
catch (Exception exception)
{
Debug.LogErrorFormat("Error writing WorldLocking diagnostics record: {0}", exception);
}
// The basic output from the FrozenWorld engine (current fragment and its alignment)
// are applied to the unity scene
FragmentManager.Update(AutoRefreeze, AutoMerge);
/// The following assumes a camera hierarchy like this:
/// Nodes_A => AdjustmentFrame => Nodes_B => camera
/// The cumulative effect of Nodes_B is to transform from Spongy space to playspace.
/// Spongy space is the space that the camera moves about in, and is the space that
/// coordinates coming from scene agnostic APIs like XR are in.
/// (Note the MRTK APIs are in Unity's global space, not Spongy space.
/// The internal structure of that graph is inconsequential here, the only dependency
/// is on the cumulative transform, PlayspaceFromSpongy.
/// Likewise, the cumulative effect of Nodes_A is to transform from alignment space (described below)
/// to Unity's global space, referred to here as FrozenSpace.
/// The AdjustmentFrame's transform is composed of two transforms.
/// The first comes from the FrozenWorld engine DLL as the inverse of Plugin.GetAlignment(),
/// and transforms from Playspace to the base stable world locked space, labeled as
/// LockedFromPlayspace.
/// The second transforms from this stable but arbitrary space to a space locked
/// to a finite set of real world markers. This transform is labeled PinnedFromLocked.
/// The transform chain equivalent of the above camera hierarchy is:
/// FrozenFromPinned * [PinnedFromLocked * LockedFromPlayspace] * PlayspaceFromSpongy * SpongyFromCamera
///
/// FrozenFromSpongy and its inverse are useful for converting between the coordinates of scene agnostic APIs (e.g. XR)
/// and Frozen coordinates, i.e. Unity's global space.
/// FrozenFromLocked is convenient for converting between the "frozen" coordinates of the FrozenWorld engine DLL
/// and Unity's global space, i.e. Frozen coordinate.
if (Enabled)
{
Pose playspaceFromLocked = Plugin.GetAlignment();
LockedFromPlayspace = playspaceFromLocked.Inverse();
SpongyFromCamera = Plugin.GetSpongyHead();
Pose lockedHeadPose = LockedFromPlayspace.Multiply(PlayspaceFromSpongy.Multiply(SpongyFromCamera));
alignmentManager.ComputePinnedPose(lockedHeadPose);
PinnedFromLocked = alignmentManager.PinnedFromLocked;
}
else
{
SpongyFromCamera = Camera.main.transform.GetLocalPose();
/// Note leave adjustment and pinning transforms alone, to facilitate
/// comparison of behavior when toggling FW enabled.
}
AdjustmentFrame.SetLocalPose(PinnedFromLocked.Multiply(LockedFromPlayspace));
AutoSaveTriggerHook();
}
private void ComputeIkChainIk(Pose target_pose, Armature rest_armature, Armature morphed_armature, VpdPose vpd_pose, string ik_bone_name, IKArmature ik_armature, string root_name, string end_effector_name)
{
var root_bone = bonesByName[root_name];
var root_position = morphed_armature.GetJointWorldPosition(root_bone.Name);
var root_orientation = morphed_armature.GetJointWorldOrientation(root_bone.Name);
var end_effector_bone = bonesByName[end_effector_name];
var end_effector_position = morphed_armature.GetJointWorldPosition(end_effector_name);
var ik_bone = bonesByName[ik_bone_name];
var goal_position = GetMorphedWorldPosition(ik_bone.Index, vpd_pose);
var goal_orientation = GetMorphedWorldOrientation(ik_bone.Index, vpd_pose);
if (new[] { "leg_ik.R", "leg_ik.L", "tiptoe_ik.R", "tiptoe_ik.L" }.Contains(ik_bone_name))
{
// calculate the rotation need to rotate the vector
// (end_effector - root) to (goal - root)
var root_to_end_effector = end_effector_position - root_position;
var root_to_end_effector_length = root_to_end_effector.Length;
var root_to_goal = goal_position - root_position;
var root_to_goal_length = root_to_goal.Length;
var dot_prod = Vector3D.DotProduct(root_to_end_effector, root_to_goal);
if (root_to_end_effector_length > 0.00001)
{
dot_prod /= root_to_end_effector_length;
}
if (root_to_goal_length > 0.00001)
{
dot_prod /= root_to_goal_length;
}
if (Math.Abs(dot_prod - 1) > 0.0001)
{
var cos_theta = dot_prod;
var theta = Math.Acos(cos_theta);
var axis = Vector3D.CrossProduct(root_to_end_effector, root_to_goal);
axis.Normalize();
var rotation = new Quaternion(axis, theta * 180 / Math.PI);
root_orientation = rotation * root_orientation;
}
}
var chain_rest_armature = ik_armature.GetRestArmature();
chain_rest_armature.SetJointParameter(root_name,
(Vector3D)root_position,
root_orientation);
// create a pose with joint changes
// derived from the VPD pose.
var chain_normal_pose = new Pose();
foreach (var ik_joint in ik_armature.GetIkJoints())
{
var ik_joint_name = ik_joint.Name;
chain_normal_pose.SetJointChange(
ik_joint_name,
target_pose.GetJointChange(ik_joint_name));
}
chain_normal_pose.SetJointChange(root_name, new JointChange());
var chain_ik_pose = new IKPose();
chain_ik_pose.SetNormalPose(chain_normal_pose);
chain_ik_pose.EndEffectorGoals.SetEndEffectorPosition(end_effector_name, goal_position);
var result_pose = new Pose();
ik_armature.SolveForPose(result_pose, chain_ik_pose, 30, 0.0001);
foreach (var ik_joint in ik_armature.GetIkJoints())
{
var joint_change = result_pose.GetJointChange(ik_joint.Name);
target_pose.SetJointChange(ik_joint.Name, joint_change);
}
if (new[] { "leg_ik.R", "leg_ik.L", "tiptoe_ik.R", "tiptoe_ik.L" }.Contains(ik_bone_name))
{
// Fix the root joint change so that it is given
// with respect to its parent.
var root_parent_orientation = morphed_armature.GetJointWorldOrientation(bones[bonesByName[root_name].ParentIndex].Name);
root_parent_orientation.Invert();
var fixed_root_orientation = root_parent_orientation * root_orientation * result_pose.GetJointChange(root_name).Orientation;
target_pose.SetJointChange(root_name,
new JointChange(new Vector3D(0, 0, 0), fixed_root_orientation));
}
rest_armature.Morph(morphed_armature, target_pose);
}
public void HandleTrackableUpdate(Pose pose, string trackableId)
{
_value = pose;
_scanUI.ShowGizmo(pose);
}
/// <summary>
/// Gets the current animated skeleton pose. Will allocate the bone transformation array memory or reuse the cached one.
/// </summary>
/// <param name="pose">The output pose.</param>
public void GetCurrentPose(ref Pose pose)
{
Internal_GetCurrentPose(unmanagedPtr, ref pose);
}
/// <summary>
/// Co-routine to lerp the controller to crouch size
/// </summary>
private IEnumerator ControllerToCrouching()
{
// Get the target height from the heightCrouch trigger
float fFinishHeight = m_HeightCrouch.GetComponent <CapsuleCollider>().height;
// Wait until it is clear.
// This should only be colliding if character is prone and under an object
while (m_HeightCrouch.Colliding)
{
if (m_HeightCrouch.OnlyDynamicCollisions)
{
m_HeightCrouch.ForceOutRigidbodies();
}
yield return(new WaitForEndOfFrame());
}
// Store the previous pose
m_ePreviousPose = m_ePose;
// Set the current pose
m_ePose = Pose.Crouch;
// Initialise timer
m_fPoseTimer = 0.0f;
// Set the lerping flag
m_bPoseLerping = true;
// Current height is stored as the start height as to lerp from any beginning
float fStartHeight = m_CharacterController.height;
// Continue to lerp towards target height while unobstructed
while (m_fPoseTimer < m_fPoseTime)
{
// If new collisions detected, wait.
if (m_HeightCrouch.Colliding)
{
if (m_HeightCrouch.OnlyDynamicCollisions)
{
m_HeightCrouch.ForceOutRigidbodies();
}
yield return(new WaitForEndOfFrame());
}
else
{
// We track the previous and new lerps in order to calculate the lifting of legs in a crouch jump
float previousLerp = Mathf.Lerp(fStartHeight, fFinishHeight, m_fPoseTimer / m_fPoseTime);
// Update pose timer
m_fPoseTimer += Time.deltaTime;
float newLerp = Mathf.Lerp(fStartHeight, fFinishHeight, m_fPoseTimer / m_fPoseTime);
// Update height and centre
m_CharacterController.height = newLerp;
m_CharacterController.center = Vector3.up * m_CharacterController.height * 0.5f;
m_POVCamera.transform.parent.localPosition = Vector3.up * (1 - m_fEyeDepthRatio) * m_CharacterController.height;
// If crouching in the air, Lift the legs by moving the whole collider up.
if (m_eJump != Jump.Grounded)
{
m_CharacterController.Move(Vector3.up * (previousLerp - newLerp));
}
yield return(new WaitForEndOfFrame());
}
}
// Once finished, snap to height, centre, and camera, to remove any inaccuracy
m_CharacterController.height = fFinishHeight;
m_CharacterController.center = Vector3.up * m_CharacterController.height * 0.5f;
m_POVCamera.transform.parent.localPosition = Vector3.up * (1 - m_fEyeDepthRatio) * m_CharacterController.height;
m_fPoseTimer = 0.0f;
m_bPoseLerping = false;
}
