void _HandleTwists() { Twist resultGesture = Twist.None; float resultAngle = 0; if (Input.touchCount == 0 && isGestureGoing) { isGestureGoing = false; resultGesture = Twist.Ending; } if (Input.touchCount == 2) { var touch0 = Input.GetTouch(0); var touch1 = Input.GetTouch(1); var touch0Direction = touch0.position - touch0.deltaPosition; var touch1Direction = touch1.position - touch1.deltaPosition; if (touch1Direction != touch1.position) { resultAngle = Vector3.SignedAngle(touch1.position - touch0.position, touch1Direction - touch0Direction, Camera.main.transform.forward); resultAngle = (Camera.main.transform.forward.z < 0 ? resultAngle * -1 : resultAngle); resultGesture = Mathf.Abs(resultAngle) > minTwistAngle ? Twist.Twist : Twist.None; } } if (resultGesture != Twist.None && _IsActionPermited(resultGesture)) { isGestureGoing = (resultGesture != Twist.Ending); _NotifyAboutTwist(resultGesture, resultAngle); } }
public Twist limit (Twist input) { Twist output = new Twist (); output.linear = linear.limit ( input.linear ); output.angular = angular.limit ( input.angular ); return output; }
public FrameVel(Frame _T, Twist _t) : this(kdlPINVOKE.new_FrameVel__SWIG_2(Frame.getCPtr(_T), Twist.getCPtr(_t)), true) { if (kdlPINVOKE.SWIGPendingException.Pending) { throw kdlPINVOKE.SWIGPendingException.Retrieve(); } }
public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex) { int arraylength = -1; bool hasmetacomponents = false; object __thing; int piecesize = 0; byte[] thischunk, scratch1, scratch2; IntPtr h; //link_name link_name = ""; piecesize = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex); currentIndex += 4; link_name = Encoding.ASCII.GetString(SERIALIZEDSTUFF, currentIndex, piecesize); currentIndex += piecesize; //pose pose = new Pose(SERIALIZEDSTUFF, ref currentIndex); //twist twist = new Twist(SERIALIZEDSTUFF, ref currentIndex); //reference_frame reference_frame = ""; piecesize = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex); currentIndex += 4; reference_frame = Encoding.ASCII.GetString(SERIALIZEDSTUFF, currentIndex, piecesize); currentIndex += piecesize; }
public override void Randomize() { int arraylength = -1; Random rand = new Random(); int strlength; byte[] strbuf, myByte; //header header = new Header(); header.Randomize(); //pose pose = new Pose(); pose.Randomize(); //twist twist = new Twist(); twist.Randomize(); //success success = rand.Next(2) == 1; //status_message strlength = rand.Next(100) + 1; strbuf = new byte[strlength]; rand.NextBytes(strbuf); //fill the whole buffer with random bytes for (int __x__ = 0; __x__ < strlength; __x__++) { if (strbuf[__x__] == 0) //replace null chars with non-null random ones { strbuf[__x__] = (byte)(rand.Next(254) + 1); } } strbuf[strlength - 1] = 0; //null terminate status_message = Encoding.ASCII.GetString(strbuf); }
protected override bool Loop() { Twist twist = new Twist(); twist.Linear.X = 1f; Input = twist; Mat cam1 = cameraThread.Image1; if (cam1 != null) { Console.WriteLine("Camera 1: {0} x {1}", cam1.Width, cam1.Height); } if (!Sleep(1000)) { return(false); } twist.Linear.X = 0f; Input = twist; Mat cam2 = cameraThread.Image2; if (cam2 != null) { Console.WriteLine("Camera 2: {0} x {1}", cam2.Width, cam2.Height); } return(Sleep(1000)); }
public TwistVel(Twist p) : this(kdlPINVOKE.new_TwistVel__SWIG_3(Twist.getCPtr(p)), true) { if (kdlPINVOKE.SWIGPendingException.Pending) { throw kdlPINVOKE.SWIGPendingException.Retrieve(); } }
public static TwistSwing Decompose(CartesianAxis twistAxis, Quaternion q) { DebugUtilities.AssertIsUnit(q); float w = q.W; float x = q[(int)twistAxis]; float y = q[((int)twistAxis + 1) % 3]; float z = q[((int)twistAxis + 2) % 3]; float swingW = (float)Sqrt(Sqr(w) + Sqr(x)); float twistW, twistZ; if (swingW != 0) { twistW = w / swingW; twistZ = x / swingW; } else { //if swingW is 0, then there isn't a unique decomposition, so I'll arbitrarily assume no twist twistW = 1; twistZ = 0; } float swingY = twistW * y - twistZ * z; float swingZ = twistW * z + twistZ * y; var twist = new Twist(Sign(twistW) * twistZ); var swing = new Swing(swingY, swingZ); return(new TwistSwing(twist, swing)); }
public LinkState(string link_name, Pose pose, Twist twist, string reference_frame) { this.link_name = link_name; this.pose = pose; this.twist = twist; this.reference_frame = reference_frame; }
public void setBodyTwist(Twist body_twist) { lock ( mutex_ ) { UnityEngine.Quaternion orientation = new UnityEngine.Quaternion((float)orientation_.x, (float)orientation_.y, (float)orientation_.z, (float)orientation_.w); orientation = UnityEngine.Quaternion.Inverse(orientation); UnityEngine.Vector3 body_linear = new UnityEngine.Vector3((float)body_twist.linear.x, (float)body_twist.linear.y, (float)body_twist.linear.z); UnityEngine.Vector3 world_linear = orientation * body_linear; // Eigen::Quaterniond orientation(orientation_.w, orientation_.x, orientation_.y, orientation_.z); // Eigen::Matrix<double,3,3> inverse_rotation_matrix(orientation.inverse().toRotationMatrix()); // // Eigen::Vector3d body_linear(body_twist.linear.x, body_twist.linear.y, body_twist.linear.z); // Eigen::Vector3d world_linear(inverse_rotation_matrix * body_linear); twist_.linear.x = world_linear.x; twist_.linear.y = world_linear.y; twist_.linear.z = world_linear.z; UnityEngine.Vector3 body_angular = new UnityEngine.Vector3((float)body_twist.angular.x, (float)body_twist.angular.y, (float)body_twist.angular.z); UnityEngine.Vector3 world_angular = orientation * body_angular; // Eigen::Vector3d body_angular(body_twist.angular.x, body_twist.angular.y, body_twist.angular.z); // Eigen::Vector3d world_angular(inverse_rotation_matrix * body_angular); twist_.angular.x = world_angular.x; twist_.angular.y = world_angular.y; twist_.angular.z = world_angular.z; } }
public override int GetHashCode() { unchecked { int hashCode = (int)EventType; hashCode = (hashCode * 397) ^ (int)DeviceType; hashCode = (hashCode * 397) ^ (int)PointerId; hashCode = (hashCode * 397) ^ Position.GetHashCode(); hashCode = (hashCode * 397) ^ Timestamp.GetHashCode(); hashCode = (hashCode * 397) ^ (int)KeyModifiers; hashCode = (hashCode * 397) ^ ContactRect.GetHashCode(); hashCode = (hashCode * 397) ^ IsBarrelButtonPressed.GetHashCode(); hashCode = (hashCode * 397) ^ IsCanceled.GetHashCode(); hashCode = (hashCode * 397) ^ IsEraser.GetHashCode(); hashCode = (hashCode * 397) ^ IsHorizontalMouseWheel.GetHashCode(); hashCode = (hashCode * 397) ^ IsInRange.GetHashCode(); hashCode = (hashCode * 397) ^ IsInverted.GetHashCode(); hashCode = (hashCode * 397) ^ IsLeftButtonPressed.GetHashCode(); hashCode = (hashCode * 397) ^ IsMiddleButtonPressed.GetHashCode(); hashCode = (hashCode * 397) ^ IsRightButtonPressed.GetHashCode(); hashCode = (hashCode * 397) ^ IsXButton1Pressed.GetHashCode(); hashCode = (hashCode * 397) ^ IsXButton2Pressed.GetHashCode(); hashCode = (hashCode * 397) ^ IsPrimary.GetHashCode(); hashCode = (hashCode * 397) ^ MouseWheelDelta; hashCode = (hashCode * 397) ^ Orientation.GetHashCode(); hashCode = (hashCode * 397) ^ TouchConfidence.GetHashCode(); hashCode = (hashCode * 397) ^ Twist.GetHashCode(); hashCode = (hashCode * 397) ^ XTilt.GetHashCode(); hashCode = (hashCode * 397) ^ YTilt.GetHashCode(); hashCode = (hashCode * 397) ^ (int)PointerUpdateKind; return(hashCode); } }
// set pose/twist relative to the frame of this entity (Body/Model) // leave empty or "world" or "map" defaults to world-frame public ModelState() { this.model_name = ""; this.pose = new Pose(); this.twist = new Twist(); this.reference_frame = ""; }
public ModelState(string model_name, Pose pose, Twist twist, string reference_frame) { this.model_name = model_name; this.pose = pose; this.twist = twist; this.reference_frame = reference_frame; }
// set pose/twist relative to the frame of this link/body // leave empty or "world" or "map" defaults to world-frame public LinkState() { this.link_name = ""; this.pose = new Pose(); this.twist = new Twist(); this.reference_frame = ""; }
public void TestAsQuaternion() { var twist = new Twist(0.4f); var swing = new Swing(0.2f, 0.3f); var twistSwing = new TwistSwing(twist, swing); MathAssert.AreEqual( twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)), twistSwing.AsQuaternion(CartesianAxis.X), Acc); MathAssert.AreEqual( twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)), twistSwing.AsQuaternion(CartesianAxis.X), Acc); MathAssert.AreEqual( twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)), twistSwing.AsQuaternion(CartesianAxis.X), Acc); MathAssert.AreEqual( twist.AsQuaternion(CartesianAxis.X).Chain(swing.AsQuaternion(CartesianAxis.X)), twistSwing.AsQuaternion(CartesianAxis.X), Acc); }
//[System.Diagnostics.DebuggerStepThrough] public override void Deserialize(byte[] SERIALIZEDSTUFF, ref int currentIndex) { int arraylength = -1; bool hasmetacomponents = false; int piecesize = 0; byte[] thischunk, scratch1, scratch2; IntPtr h; object __thing; //header header = new Header(SERIALIZEDSTUFF, ref currentIndex); //pose pose = new Pose(SERIALIZEDSTUFF, ref currentIndex); //twist twist = new Twist(SERIALIZEDSTUFF, ref currentIndex); //success success = SERIALIZEDSTUFF[currentIndex++] == 1; //status_message status_message = ""; piecesize = BitConverter.ToInt32(SERIALIZEDSTUFF, currentIndex); currentIndex += 4; status_message = Encoding.ASCII.GetString(SERIALIZEDSTUFF, currentIndex, piecesize); currentIndex += piecesize; }
private Twist TranslateTwist() { Twist twist = new Twist(); twist.Linear.X = controller.LeftThumbstick.Y; return(twist); }
public bool update (Twist command) { if ( !connected () ) return false; command.linear.z = get (); return true; }
public bool update (Twist command) { if ( !connected () ) return false; getCommand ( ref command.linear.x, ref command.linear.y ); return true; }
public void setTwist(Twist twist) { lock ( mutex_ ) { twist_ = twist; } }
public static Twist Twist(Random rnd) { var twistX = ContinuousUniform.Sample(rnd, -1, 1); var twist = new Twist((float)twistX); return(twist); }
public void setColumn(uint i, Twist t) { kdlPINVOKE.Jacobian_setColumn(swigCPtr, i, Twist.getCPtr(t)); if (kdlPINVOKE.SWIGPendingException.Pending) { throw kdlPINVOKE.SWIGPendingException.Retrieve(); } }
/// <summary> /// Extends the type <c>Twist</c> with a method <c>ToTwistMessage</c>, which creates an twist message. /// </summary> /// <param name="twist">Twist</c> object to be extended</param> /// <returns>Returns an instance of <c>geometry_msgs.Twist</c>.</returns> public static geometry_msgs.Twist ToTwistMessage(this Twist twist) { return(new geometry_msgs.Twist() { linear = twist.Linear.ToVector3Message(), angular = twist.Angular.ToVector3Message() }); }
public OdomSubscriberHelper (NodeHandle nh, string topic, Pose pose, Twist twist, Accel acceleration, Header header) { pose_ = pose; twist_ = twist; acceleration_ = acceleration; header_ = header; odom_sub_ = nh.subscribe<Odometry> ( topic, 1, odomCallback ); }
public GetModelStateResponse(Header header, Pose pose, Twist twist, bool success, string status_message) { this.header = header; this.pose = pose; this.twist = twist; this.success = success; this.status_message = status_message; }
private void TestClampRotation(Vector3 input, Vector3 expected) { var inputTS = new TwistSwing(Twist.MakeFromAngle(input.X), Swing.MakeFromAxisAngleProduct(input.Y, input.Z)); var clampedTS = constraint.Clamp(inputTS); var expectedTS = new TwistSwing(Twist.MakeFromAngle(expected.X), Swing.MakeFromAxisAngleProduct(expected.Y, expected.Z)); MathAssert.AreEqual(expectedTS, clampedTS, 1e-4f); }
public void TestCalculateDelta() { var initialTwist = Twist.MakeFromAngle(0.2f); var finalTwist = Twist.MakeFromAngle(0.8f); var deltaTwist = Twist.CalculateDelta(initialTwist, finalTwist); MathAssert.AreEqual(Twist.MakeFromAngle(0.6f), deltaTwist, Acc); }
// comments if available public GetModelStateResponse() { this.header = new Header(); this.pose = new Pose(); this.twist = new Twist(); this.success = false; this.status_message = ""; }
private void SendStopMessage() { Twist message = new Twist(); message.linear = stopLinear; message.angular = stopAngular; cmdPublisher.Publish(message); }
public void TestApplyDelta() { var initialTwist = Twist.MakeFromAngle(0.5f); var deltaTwist = Twist.MakeFromAngle(0.3f); var finalTwist = Twist.ApplyDelta(initialTwist, deltaTwist); MathAssert.AreEqual(Twist.MakeFromAngle(0.8f), finalTwist, Acc); }