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; }